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ABSTRACT 


The recognition of underwater objects and obstacles by sonar has been explored in 
many forms, particularly through the use of high-resolution imaging sonar systems. This 
work explores a method of providing real-time obstacle avoidance and navigational 
position updating for an Autonomous Underwater Vehicle (AUV) by applying regression 
analysis and geometric interpretation to sonar range data obtained from a low-cost, low- 
resolution, fixed-beam sonar. The algorithm utilized by this method first develops a least- 
squares fit for sonar range data in a 2-D manner. The parameters developed by this method 
are then compared to an environmental model for position identification. If no match 1s 
achieved, then by applying the known geometry of the acoustic signal, an estimate for a 3- 
D surface is derived. This derived 3-D surface is then added to the environmental model to 
enable accurate path planning and post-mission analysis information. This method 15 
currently implemented on an operational AUV operating in a well-defined orthogonal 
environment at NPS. The paper also discusses the simulation of the sonar systems using a 
ray tracing technique in a real-time dynamic graphical simulation implemented on a Silicon 


Graphics IRIS workstation. 
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I. INTRODUCTION 


A. MOTIVATION FOR AUV КЕБЕАКСИ 


Remotely operated air and land vehicles have been developed to extend the range of 
sensors or weapons of military units. The Israeli Air Force utilized remotely piloted 
vehicles (RPVs) for reconnaissance and electronic intelligence collection prior to the 
successful air strikes in the Bekka Valley in 1982. This utilization of RPVs as tactical 
probes resulted in zero losses to the Israeli Air Force, and near-total destruction of the 
Syrian Air Force [Ref. 1]. 

Current Maritime Strategy emphasizes the U.S. Navy’s role in a forward area strategy 
conducting operations in or near enemy waters [Ref. 2]. Given the Soviet Navy’s “bastion” 
defense strategy, utilizing multiple layers of defensive units to protect strategic missile 
submarines, an underwater vehicle could serve as an effective tactical probe. Due to the 
long-range aspect of such a mission and the presence of numerous enemy units, the 
presence of a "mother" platform providing a radio control link is not feasible. An 
autonomous underwater vehicle (AUV) with onboard sensors and control units, capable of 
carrying out a preplanned mission without external guidance, has been shown to be of 
potential military value and cost effectiveness [Ref. 1]. 

While the attention given to Soviet and Warsaw Pact military strategies may have 
lessened due to the decline of Communism in Eastern Europe and the Soviet Union, events 
in and around the Persian Gulf have validated the need for truly autonomous vehicles to 


assist in effecting the Maritime Strategy. 


Numerous missions are conceivable for an AUV. Some typical missions might include 
the following: 


e Tactical probe 

e Covert surveillance 

e Mine warfare 

e Weapons delivery 

e Underwater terrain mapping. 


Equipped with the appropriate sensors and manipulators, an AUV could carry out these 


missions without exposing tactical units or personnel to danger, and for a lower cost. 


B. AUV CONCEPT 


In the simplest form, an AUV is an unmanned submersible vehicle with onboard 
systems and sub-systems that provide motive power, motion control, navigation, obstacle 


detection and collision avoidance. To be truly autonomous, the vehicle should be able to 
execute a planned mission by controlling and monitoring the onboard systems without any 
external input. It should be able to replan its mission in the event of internal anomalies, such 
as sub-system degradation, and it should have the capability of replanning its path to avoid 
previously unknown obstacles [Ref. 3]. While the above qualities outline the minimum re- 
quirements for a simple AUV, a realistic mission-capable AUV would necessarily carry 
additional, possibly specialized, mission-dependent sub-systems and sensors. The Naval 


Postgraduate School (NPS) AUV II is designed to model the simple AUV concept. 


C. OBSTACLE AVOIDANCE 


The concept of obstacle avoidance for an AUV entails more than simply turning the 


vehicle in a predefined manner to avoid a collision. In the context of an AUV conducting 

a predefined mission, the concept of obstacle avoidance carries the implicit notion that the 
AUV will attempt to continue its mission by replanning its path around the obstacle. This 
further implies two additional capabilities, the ability to consult a stored model of the en- 


vironment, and the ability to detect, recognize, and quantify previously unknown obstacles 


so that they may be added to the stored model [Ref. 3]. This can be simplified into the fol- 
lowing steps: 


* Detect the obstacle. 

* Perform a safety maneuver to avoid the obstacle. 

* Perform other maneuvers to extract obstacle features. 

* Add the obstacle to the stored environmental model. 

* Replan the path to avoid the obstacle while continuing the mission. 


This thesis will address all but the final step. Work on the path replanning problem has been 


addressed by other NPS AUV II project team members [Ref. 4]. 


D. OBJECTIVES 


This thesis will address the following research questions: 


* How can data from ultrasonic sensors be best utilized to recognize obstacles 
during operation of the NPS AUV II? 

* What is the optimal configuration for ultrasonic sensors on the AUV II to 
provide obstacle detection and terrain data collection for post-mission 
analysis? 

* What type of motion algorithm will best provide collision avoidance and 
obstacle feature extraction? 

* How can sensor data be utilized in post-mission analysis to generate a 3-D 
terrain model? 


This research was designed to move the NPS AUV II project into its next phase of devel- 
opment by providing a stable test platform capable of maintaining a collision-free path 
while conducting missions in its current environment. the NPS swimming pool. At the out- 
set of this research, the AUV II had no sensors installed and was capable of performing only 


simple, open-loop missions in the swimming pool. 


E. THESIS ORGANIZATION 


Subsequent chapters of this work will address the research questions posed above. 


Chapter II describes the NPS AUV II in terms of its physical characteristics as well as its 


control and software hierarchy. This section describes previous work done on this project. 


Chapter II also examines the most current work on sonars and AUVs. Chapter III provides 
the theoretical framework for sonar system operation, the physical and electrical character- 
isucs of the sonar chosen for the NPS AUV II, and some of the problems inherent with these 
systems and their installation. 


Chapter IV discusses the extraction of linear features of obstacles by means of a least 
squares fit algorithm. The algorithm and the particular tests performed on sonar range 
inputs are examined, also. Chapter V covers the utilization of the information developed by 
linear feature extraction by the collision avoidance system and other processes. 
Experimental results are presented in Chapter VI. Pre-mission simulation and post-mission 
analvsis and 3-D displav of mission data 1s discussed in Chapter VII. Finally, conclusions 
regarding the viability of this particular approach to collision. avoidance and 


recommendations for further study are presented in Chapter VIII. 


П. NPS AUV PROJECT AND RELATED AUV WORK 


The Naval Postgraduate School’s AUV project was started in 1987, and involves 
personnel from the Mechanical Engineering, Computer Science, and Electrical 
Engineering departments. The project has evolved through design and feasibility studies, 
the construction and testing of the NPS AUV I radio-controlled model vehicle, the 
construction of the AUV II vehicle, and its ongoing testing and development. [Refs. 5, 6, 7] 
A. THE NPS AUV II VEHICLE 

1. Vehicle Characteristics 

The basic layout of equipment for the AUV II vehicle is illustrated in Figure 2-1. 
The vehicle design has been detailed by Good [Ref. 8]. The main vehicle body is 
constructed as an aluminum box with a beam of 16 inches, a height of 10 inches, and a 
length of 72 inches. The nose cone is constructed of fiberglass and extends the overall 
vehicle length to 92 inches. The vehicle uses fixed ballast and displaces approximately 390 
pounds. Vehicle control is provided by eight independently driven control surfaces, four 
tunnel thrusters, and two main drive motors. The counter-rotating 24 volt DC drive motors 
power four-inch propellers and provide one-eighth horsepower each, driving the AUV at a 
maximum speed of about two knots (3 feet per second). The control surfaces provide a high 
degree of maneuverability, with a minimum turning diameter of approximately 20 ft., less 
than three ship lengths. All of the installed systems are powered by lead-acid gel batteries 
capable of providing power for up to two and one-half hours. 


The control and guidance software processes run on a GESPAC MPU 20HF processor 
with a Motorola 68020 CPU and 68881 math coprocessor running at 16 MHz. The system 


has 2.5 Mb of RAM and runs the OS-9 multi-tasking operating system. Input and output 


between the CPU and the installed systems is routed through two GESDAC-2B 8-channel 12 bit 
Digital-to-Analog/Analog-to-Digital (DA/AD) converter cards and a GESPIA-3A parallel 
interface board. Serial communications with external systems is achieved via a 2400 bps modem. 
The navigation system sensor Suite includes a flux gate compass and directional gyroscope, a 
vertical gyroscope system, and a three axis rate gyroscope system with translational 
accelerometers. A paddlewheel speed sensor is installed in the nose, along with the four sonar 


transducers. 


Battery 
Pack 


TOP VIEW 


Sonar transducers Tunnel thrusters Drive motors 


SIDE VIEW 





Figure 2-1. NPS AUV H Vehicle Component Layout. 


2. Software Hierarchy 

A block diagram illustrating the interaction of systems and processes 1s given in 
Figure 2-2 [Ref. 9]. The dataflow diagram in Figure 2-3 represents the software 
instantiation of the processes shown in Figure 2-2. The individual data items are further 
described in the Data Dictionary located in Appendix A. The mission plan and the 
particular environmental database for the operating area are downloaded to the AUV from 
the mission support system, running on a GRIDCASE 386 laptop computer, via the 
modem. Referring now to Figure 2-2, the mission executor oversees the mission execution 
by providing geographic waypoints and tasks to the guidance system. The guidance system 
provides desired vehicle postures, (x, y, z, 9), as heading, speed, and depth commands to 
the autopilot system. The autopilot, updated by the navigation system, controls vehicle 
systems to achieve the desired postures. Vehicle systems are monitored for possible 
problems, such as the loss of a control servo, that might necessitate mission replanning. 

The sonar systems provide range data that are used to detect obstacles and to 
develop obstacle features. The pattern recognition system attempts to match obstacle 
features with known obstacles in the database in order to provide position updates to the 
navigation system. Those obstacles that are not found in the database are added to the 
model, and the obstacle avoidance decision maker is signalled to take possible evasive 
measures. Details of the sonar processes are discussed in Chapter V. Additionally, the 
mission replanner is signalled to develop a new path utilizing the updated environmental 


database model. All of the processes running on the GESPAC are coded in C. 
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Figure 2-2. NPS AUV II 
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B. RELATED WORK 
1. Sonar Range Finding 
There are a few research reports on linear fitting on radial range data from ultrasonic 
sensors in autonomous land vehicle control. Crowley proposed the recursive linear fitting 
algorithm to find a linear segment among radial data [Ref. 10]. Drumheller proposed an 
iterative endpoint fit for the same purpose [Ref. 11]. Several reports stress the uncertainty 
and ambiguity of sonar range data [Refs. 12, 13, 14]. The method of range data fitting 
detailed in Chapter IV was tested on the autonomous mobile robot Yamabico-11 which was 
developed at the University of California at Santa Barbara and at the Naval Postgraduate 
School (NPS) by Kanayama [Ref. 15]. 
2. AUV Sonar Research 
One other AUV project has utilized a similar sonar system, and used range data 
slope information provided by the sensors to assess targets [Ref. 16]. Most work in this area 
has utilized high resolution sector-scanning, or multi-beam type sonars and image 
processing algorithms for obstacle recognition [Refs. 17, 18] and position estimation [Ref. 
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IM. ULTRASONIC TRANSDUCER PROPERTIES 
AND INSTALLATION 


A. DATASONICS PSA-900 PROGRAMMABLE SONAR ALTIMETER 
1. Physical and Electrical Characteristics 
The ultrasonic transducer chosen for use on the NPS AUV project 1s the PSA-900 
Programmable Sonar Altimeter manufactured by Datasonics, Inc. Each transducer system 
consists of a transducer head, a microprocessor-based control system, and associated 
connecting cables. Specifications for the PSA-900 are listed in Table 1. Each transducer 


head measures 2.25 inches in diameter, and 1 to 2 inches in length. 


TABLE 1-1. DATASONICS PSA-900 SPECIFICATIONS 


Operating Frequency: 175/200/223 kHz (fixed) 

Beam Pattern: 10°, conical 

Pulse Length: 350 secs 

Repetition Rate: User-selectable (10 / 1 / 0.1 pps) 
Range: User-selectable (30 / 300 meters) 


Resolution: 1 ст (Q 30 m range / 10 cm (9 300 m range 


Accuracy: + 0.25% of full scale range 


Range Output: 0 - 10 v DC, proportional to full scale range 
Power Requirement: 15-28 v@ 100 mA DC 
[Ref. 20] 





2. Theory of Operation 
A sonar operates by emitting acoustic energy at a specified frequency and duration. 
Sonar range is determined by timing the sound pulse as it travels from the transducer, 
strikes an object, and then returns to the transducer. If the speed of sound through the water 
(C;) is known, then the range (d) can be determined from the following formula: 


jp (C X AC NEN) 
= EE 


(Eq 3.1) 

The nominal speed of sound (C;) in salt water with salinity of 35%o1s 1500 meters per sec- 
ond. The AC, factor represents changes in sound velocity due to environmental factors 
such as temperature, pressure, salinity and depth. Of these factors, temperature change con- 
tributes the most to sound velocity change, with as much as a five percent net increase or 
decrease in sound velocity. To compensate for temperature related changes in sound veloc- 
ity, the PSA-900 includes a temperature sensor that enables it to make automatic 
adjustments based on actual temperature readings.The At factor in (Eq 3.1) is the error fac- 
tor in determining the actual time elapsed since the sound pulse was generated and is 
referred to as jitter. For the PSA-900 sonar this jitter error can be as small as 5 microsec- 
onds, or approximately 0.4 cm total distance [Ref. 20]. 

The ability of a sonar to detect an echo is determined by the initial pulse strength, 
the size and type of the target, the distance to the target, and other factors related to noise. 
Expressed in terms of the detection threshold (DT), or the ability of the sonar to just detect 
a target, the active sonar equation 15 

DT = Е-Е (NLD) (Eq 3.2) 
Here, SL 1s the signal level of the original pulse, 2TL is the two-way transmission path loss, 


NL 1s the background noise, and DI is the directivity index of the receiver. All terms are 


expressed in units of decibels relative to the standard reference intensity of a 1U Pa plane 


wave. The transmission loss (TL) is due to spherical spreading of the sound energy and ab- 
sorption of sound energy by particles suspended in the water. For the active sonar this 15 
expressed as 
TL = 20log2r + 2ar (Eq 3.3) 
where Œœ is the attenuation coefficient of sound in water at the frequency in use and r is the 
length of the transmission path [Ref. 21, pp. 19-23, 110-111]. With the AUV operating in 
the swimming pool environment, the noise term (NL) can be disregarded. To compensate 
for the transmission loss expressed in (Eq 3.3), the PSA-900 utilizes a time varying gain 
(TVG) amplifier to enhance signal detection capability. This TVG circuitry increases the 
gain of the receiver amplifier as a function of time to ensure that weak echoes are detected 
"er 29]. 
3. Side Lobe Effects 

The PSA-900 transducer is a circular plane type and produces a main acoustic beam 
lobe that extends approximately 15 degrees around the centerline, producing a circular 
pattern. Additionally, the transducer produces significant side lobes at approximately 25 
and 50 degrees around the centerline. Any objects in the area of the side lobes can produce 
echoes as though the object were along the centerline of the sonar. These false returns can 
produce erroneous results unless corrected or eliminated. 

Since it 1s impossible to alter the physical characteristics of the sonar beam, the 
method of attempting to eliminate the side lobe returns focused on the design of a shield to 
be placed on each transducer. The shield is in the form of a conic section, measuring 2.5 in. 
in length, with an angle of 5? on either side of the centerline. This shield deflects the initial 
side lobe pattern and blocks sonar returns outside of 5° of the sonar’s extended centerline. 


Figure 3-1 illustrates the effect of the shield. 


Да 


Transducer 


meters 





Figure 3-1. 2-D View of the Shielded Sonar Beam. 


4. Interference Problems 
Preliminary investigations using these sonars revealed that simultaneous operation 
of two sonar systenis, both operating in a self-keying mode with different frequencies, 
could result in interference and erroneous range readings. Previous tests with both systems 
keyed simultaneously via the external keying signal input showed that mutual interference 
should not be a problem [Ref. 22, pp. 28-33]. Post-mission data analysis of tests conducted 
with two sonars, mounted orthogonally in the AUV’s nose and keyed simultaneously, 
revealed no evidence of mutual interference. 
5. Sonar Board Averaging 
The PSA-900 processor maintains a sliding window average consisting of the last 
four ranges. This average is used to determine the validity of range signals, with signals 
differing from the average by more than 10% of the maximum selected range (3 meters) 
considered to be in error. Due to this on-board averaging, when a different sonar transducer 
is selected for use with the same board, a number of pings must be conducted to “wash out” 


the effects of the previous average. Test results revealed that a minimum of 15 pings must 


14 


be generated before the new average settled at the correct range (Figure 3-2). With a ping 
rate of 10 Hz, this reduces the effective data acquisition rate to less than 0.3 Hz for each 
transducer operating on the same board. While this rate is acceptable for some missions, it 
is too low for purposes of control or obstacle avoidance. 
6. Noise Filtering 

Initial tests conducted with the sonars installed in the AUV showed that the signals 
provided by the sonars contained a noticeable arnount of spurious noise. Examination of a 
number of sets of test data led to the observation that the norse tended to present itself as a 
transient spike indicating a range shorter than the actual range. Further, each spike’s first 
transition duration (rise time) was very short, with the first data point in the spike being 15 
- 20 units less than the previous valid data point. The spikes average duration was 1 second. 
At a 10 Hz data rate, this means that ten consecutive data points may be invalid. An 
enlarged section of an unfiltered data set from the bottom sonar is seen in Figure 3-3. 


The analysis of the noise present in the signals led to the development of a digital filter 
that screens out points more than 15 units away from the current average value. Points that 
pass this screen are included in the updated moving average, consisting of the ten most 
recent valid points. In order to prevent the loss of possibly valid data representing a 
significant change in a feature’s topography, points screened out are saved in a buffer. If a 
sequence of ten points are screened out, then those ten points are used to develop a new 
average, and the buffer is cleared. When dealing with the forward sonar, the screening must 
allow for changes in the range signal due to the forward motion of the AUV at its current 


speed. Typically, this results in a screen of 25 - 30 units, rather than the 20 used for the side 


and bottom sonars. The data set seen in Figure 3-3 is shown in Figure 3-4 after filtering. 





Time (secs) —» 


Figure 3-2. Illustration of Sonar Board Averaging. One sonar board has been 
switched between two transducers every 2 seconds (20 pings). Ranges for one 
head are shown by dots, ranges for the second head are shown with asterisks. 
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Figure 3-3. Unfiltered Range Data Set. Data from the bottom so- 
nar. Range is in interface units, where 1 unit 2 0.023 ft. 
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Figure 3-4. Filtered Range Data Set. Filtered values are shown 
with **x's", original data (dots) from Figure 3-3 is also shown for 
comparison. Range is in interface units, where 1 unit = 0.023 ft. 


B. SONAR INSTALLATION 
1. AUV Nose Mount 
The four sonar transducers, with associated cones, currently installed in the AUV II 
are affixed to an aluminum mounting bracket, so that they are mutually orthogonal. There 


is a sonar oriented directly forward, directly downward, to the left, and to the right of the 
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| 
holes were cut in the nose to expose the openings of the shield cones. A thin plastic sheet. 
acoustically transparent, was placed over the openings to maintain the hvdrodvnamic prop- 
erties of the nose. The watertight cables for the sonars are connected to a watertight 
connection, mounted on the forward bulkhead of the AUV., which connects to the sonar pro- 


cessing boards. The sonar mount 1s shown in Figure 3-5. 





Figure 3-5. AUV II Sonar Installation. Shield has been removed from forward 
sonar to show transducer. Watertight connection is at upper left. 
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2. Computer Interface 

Due to power supply limitations, there are currently two sonar processing boards 
installed in the AUV I]. The tour transducers are connected to the two boards through a set 
of four microswitches, so that one board serves two transducers (Figure 3-6). Each 
microswitch is software controlled to enable the selection of a pair of sonar transducers for 
use. The switch interface to the CPU is via the GESPIA-3A parallel interface board. 
Additionally, the external keying signal is provided to each sonar board via the GESPIA- 
3A. To effect a controlled sonar ping, the appropriate pair of transducers are selected for 
connection to the sonar boards, and then the boards are keyed via the parallel interface. This 
simultaneous keying precludes the crosstalk problems discussed earlier in this chapter. 

The analog range signal (0-10 v DC) provided by the sonar processor is interfaced 
to the CPU via the GESDAC-2B analog-to-digital converter board. This interface provides 
a resolution of 4096 units, with programmable gain control. The sonar installation uses 
unity gain, with full scale representing the maximum Selected range (4096 units = 10 volts 


= 30 meters). 
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Figure 3-6. Sonar-Computer Interface Diagram. 


IV. EXTRACTION OF LINEAR FEATURES 
OF OBSTACLES 


Regression analysis is used in many applications to find linear approximations for sets 
of discrete data points. In the case of underwater obstacles and the AUV sonar, data points 
generated by sonar returns from obstacles may be used to generate linear features of the ob- 
stacles that can be used to describe the obstacles in terms of the environmental model. This 
linear feature extraction enables the system to perform pattern matching with the environ- 
mental database to allow navigational position updating, or, in the case of a previously 
unknown obstacle, the obstacle can be added to the environmental database. 

This section discusses the application of the least squares fit method to sonar data and 
the extraction of linear features of obstacles. These linear features are then described in 
terms of the AUV’s world environmental model to allow pattern matching or database up- 


date. [Ref. 23] 


A. LEAST SQUARES FIT METHOD 


1. Coordinate Transformation 
In determining global coordinates for the data points, we use the AUV’s dead 


reckoning (DR) position (x,, y,) and heading orientation, y, with respect to the global 


system, and the sensor’s orientation with respect to the AUV body (Figure 4-1). For the side 
and forward sonars, the effects due to roll and pitch are negligible and can be ignored due 
to the inherent stability of the AUV in these axes. Similarly, the bottom sonar range 15 
affected only by the pitch angle, with effects due to roll being minimal, and the range being 


independent of heading. Note that the heading angle, y, is measured in a clockwise fashion 
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Figure 4-1. World Coordinate Conversion. Diagram illustrates parameters for 
a range return from the left sonar. 


around the z axis by a gyroscope. The DR position 1s determined by 


x' = x t+ Arxux cos (y) (Eq 4.1) 


S 


Y. = yy FArxux sin (y) (Eq 4.2) 
where Ar = 0.1 secs and u is an estimate of the velocity along the vehicle's longitudinal 
velocity. This estimate for the velocity 1s a primary source of DR error. Coordinates for the 


data point. (p,, Py) from the left sonar are generated as follows: 


py E Xx range Xx cos (Миса) (Eq 4.3) 


p, ceric sin(y—x/2). (Eq 4.4) 


2. Linear Regression Principles 


We first discuss how to extract a linear feature from a set of point data by the least 
squares fit method. The linear feature is the simplest one among all the abstract geometric 
features, and is easily obtained by range sensors from an orthogonal world. Suppose a set 


R of positions for an envelope of an object in a plane is given from a range sensor. 
R= (xs уд = Ï zn.) , (Eq 4.5) 
The moments m ik of R are defined as 
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1-1 (Eq 4.6) 


Notice that mọọ = n . The centroid C of R is given by 
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The secondary moments around the centroid are given by 
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We adopt the parametric representation (r, œ) of a line with constants r and œ. If a point 
p= (x, y) satisfies an equation 
r= xcosatysina (-—n/2<aSn/2) | (Eq 4.11) 

then the point p is on a line L whose normal has an orientation Œœ and whose distance from 
the origin 1s r (Figure 4-2). This method has an advantage in expressing lines that are per- 
pendicular to the X axis. The point-slope method, where y = mx+b, 1s incapable of 
representing such a case (m — ee, b is undefined). The residual of point. p; = (x> у) апа 
еи (0) is x,cosa + y,sin&—r . Therefore, the sum of the squares of all re- 


siduals 15 
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The line which best fits the set of points 1s supposed to minimize $. Thus the optimum line 


(rœ) must satisfy 


E = = = 0, (Eq 4.13) 
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Figure 4-2. Representation of a Line. The distance from 
the point p to L is the residual. 
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where r may be negative. Substituting r 1n (Eq 4.12) by (Eq 4.15), 
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The solutions for the line parameters generated by a least squares fit are given by (Eq 4.15) 
and (Eq 4.18). 
The equivalent ellipse of inertia for the original n points is an ellipse which has the 


same moments around the center of gravity. M major AMA M minor are moments about the 


major and minor axes respectively (Figure 4-3), 
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Figure 4-3. The Equivalent Ellipse of Inertia. De- 
termined for the set of points Rk, represented by the 
line L. 


The diameters dmajor ON the major axis and dingy On the minor axis of the equivalent el- 


lipse are 


а ліло? Е AM major” oo (Eq 4.21) 
E = AM minor ™00 i (Eq 4.22) 


We define p, the ellipse thinness ratio, to be the ratio of d minor and dmajor: 


de 
= ` Ea 4.23 
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major 
A small p means a thin ellipse; as p increases toward 1, the ellipse degrades to a circle rep- 
resenting a thick line or “blob” of points. We will use p as an additional measure of the 
linearity of a set of points. 


The residual of a point p; = (xj yj) is 


0, = (ц-х)) созо + (иди) sing . (Eq 4.24) 


Therefore, the projection, p';, of the point p; onto the major axis 1s 

р'; = (x¿+0,c050, y; + Ó,sina) . (Eq 4.25) 
We will use p', and p', as estimates of the endpoints of the line segment L obtained from 
the point set R by the least squares algorithm. In a sequential fitting process such as that 
employed on the AUV, only the first point, p}, need be stored during processing. Not only 
is the equation of the line important, but the estimation of both endpoints is also valuable 


information for sensor based navigation. 


B. BEGINNING A NEW LINE SEGMENT 


While the least squares fit method provides an appropriate estimate for a line passing 
through a set of points, it does not provide a method of determining when to terminate one 
line segment and when to begin another; which is the key to this method. This decision is 
critical for determining the best linearity fit for a set of range data. For example, if we 
receive a set of points representing two perpendicular surfaces, the least squares fit method 
will generate a single line that accommodates all of the points. This fails to provide an 
accurate depiction of the surfaces’ inherent linear features. We consider a set of range data 
points representing a corner of the swimming pool. This set of data with an unterminated 
line segment fit to it is seen in Figure 4-4. A line segment terminated with the described 
method 15 seen in Figure 4-5. 

1. Test for Residuals 

To determine when to end a line segment we perform two tests on the current line. 
The first test checks the goodness of the linearity fit for the most recent. point. 
(x 


is p Jia.) - If the point satisfies 
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where c7 and c2 are positive constants (typically, c7 2 3.0 and c2 = 0.4 ft.) and the standard 


deviation, O, is 


об == | / (i-2) (Eq 4.27) 
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then the point can be included in the current line segment. 
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Figure 4-4. Unterminated Line Fit. Sonar returns from a corner 
of the swimming pool with an unterminated line fit to the points. 
Axis units in ft. 


2. Test for Ellipse Thinness 
The second test uses the ellipse information discussed above; if the thinness ratio 
for the line is smaller than the third constant, c3, then the set of points is still acceptably 
thin. We have modified the method of testing the thinness ratio from that used in [Ref. 15] 


by scaling the thinness criteria, c3, based on the length of the line. If c3 is allowed to remain 
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Figure 4-5. Terminated Line Segment. Least squares fit line seg- 

ments terminated by applying tests described in text. Here, c7 = 

3.0, c2 = 0.4, c3 = 0.1 initially. Axis units in ft. 
constant, then the ellipse thinness remains constant; however, as the line grows longer, this 
means that the acceptable width of the ellipse also grows. Using another actual data set, we 
can see in Figure 4-6 that the line is skewed slightly with respect to the predominant linear 
feature. This is due to the points representing the swimmer “pulling” the line down because 
they still fell within the thinness ratio. For larger scale features, this means that a change in 
a feature will not be recognized until it has become large enough to fall outside of the thin- 
ness criteria. As an example, a line segment that is 100 feet long, developed using an ellipse 
thinness ratio of 0.1, will have a possible width of 10 feet. This potential width determines 
the necessary minimum size for another feature to force a termination of the line segment 


based on a constant thinness ratio. 


To preclude this problem, we scale the thinness ratio as a function of length. The 
starting thinness ratio 1s determined as the desired accuracy of the smallest feature in the 
environment, with two feet being our chosen minimum usable size. Due to the physical 
dimensions of the swimming pool, the maximum length is set at 120 feet. At the small end 
of the spectrum, we have fixed the thinness ratio at 0.1, or 2.4 inches for the smallest 
features, while at the other extreme, a thickness of 1.2 feet is acceptable, which requires a 


ratio of 0.01. At intermediate lengths, the thinness ratio is determined by 


(120.0 — length) length 


D ^" A 


(Eq 4.28) 


With this adaptive ratio, the feature in Figure 4-7 is depicted in a much more realistic fash- 

ion, with the line segments falling close to the actual walls, and the swimmer being 
outlined. 

If any point fails either of the two tests described, it 15 placed into a buffer which 15 
used to initiate the next line segment. This method reduces the deleterious effects of noise, 
while maintaining the history of a possible feature change worth noting. When a line 
segment 1s terminated, the length is checked so that only those segments longer than a 


specified minimum (typically 2 ft.) will be processed by the pattern matcher. 


C. DESCRIPTION OF FEATURES IN TERMS OF AUV WORLD MODEL 


I. The Environmental Model 
Before obstacle recognition and positional updating can take place, a suitable 
environmental model must be defined. This environment model must facilitate the three 
functions of path planning, positional identification, and model updating. All three of these 
functions require the expression of the environment in some type of numerical form. 
Therefore, linear features are defined by a Cartesian coordinate system where some 


predefined point serves as the origin (see Figure 4-1 for a depiction of the pool’s coordinate 


system). In this manner, all features may be expressed in terms of their x-, y-, and 2- 


coordinates where the linking of three or more vertices defines a polyhedron. 


Line segment 


Swimmer 





Figure 4-6. Constant Ellipse Thinness Ratio. A constant value 
(c3) allows the actual width of the ellipse to grow as the line 
lengthens, producing a poor fit to the linear feature. Here cl = 
3.0, c2 = 0.3, c3 = 0.1. Axis units in feet. 


The structure used to link the points of a polyhedron is a linked list. This data 
structure 1s allocated in memory containing the x-, y-, and z-coordinates of each point. 
Pointers are then used to specify which vertices are connected to form the surfaces of a 
polyhedron. Additronally, the line parameters +, @) defining each surface are stored in the 
list, with r being the positive distance from the origin to the line, and œ being measured in 


a Clockwise fashion similar to the AUV’s heading w. 
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Figure 4-7. Adaptive Ellipse Thinness Ratio. An adaptive ratio 
(Eq 4.28) maintains a more linear fit to the data points than using 
a constant ratio such as in Figure 5, c7 = 3.0 and c2 = 0.3. Axis 
units in ft. 


2. Position Identification and Updating 
By using the parametric representation of a line (r, œ), the process of matching is a 
simple matter of comparing the generated parameters with the parameters stored in the 
environmental model. The match criteria for r and œ are designed to account for all known 
errors in generating the line segment. There are three primary errors: (1) positional 
uncertainty or navigational error, (2) inaccuracies due to the assumption that all echoes are 
along the sonar’s centerline (cosine effect), and (3) inherent sonar range error. We currently 


use an error of 3 ft. in r, and 0.34 radians (20°) in a. 
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If a generated line segment matches a known model feature with or without an error, 
then it is possible to identify the AUV’s position more precisely and to update the position 
based on the actual range to the feature, if an error exists. Uulizing the range to the feature, 
the known geometry of the sonar beam, and the angle of the sonar relative to the feature, 
an accurate normal distance can be computed. A single sonar 1s normally capable of 
updating the AUV’s position in one dimension at any given time. In the case of the left 
sonar, with the recognized feature parallel to the x-axis, the AUV’s position along the 


global y-axis would be determined as follows, 


Yauv = “feature Tange X Sin (Op 4,6). (Eq 4.29) 


If the sonar 1s in contact with a feature that 1s parallel to the y-axis, then the AUV’s along 
the x-axis would be determined as follows, 


Xauv = "feature TANSE X COS (а (Eq 4.30) 


Т 


3. Model Updating 

If no match is found in the environmental model, then we assume that the line 
segment was a result of a previously unknown feature, or obstacle. The parameters for this 
feature, developed as a planar surface, are stored as a new feature in the environmental 
model. This information may be needed for the path planning module, particularly if the 
new feature represents an obstacle to the currently planned path. Any new features that are 
added to the model will also be displayed graphically during post-mission data analysis. 
The line segment parameters (r, œ) are stored with the feature for future pattern matching: 
hence, a previously unknown obstacle becomes a known environmental feature. 

The information generated by the least squares method is in two dimensions only, 
while the AUV is operating in three dimensions. For the forward and side sonars, the two 


dimensions used are the global x and y coordinates, while for the bottom sonar the z 


uy) 
Un 


information is treated as ‘y’ data for use with the algorithm. Features seen by the side sonars 
are in the vertical plane, while those seen by the bottom sonar are in the horizontal plane. 

Figure 4-8 shows the acoustic signal pattern of each transducer as a spherical 
section. A valid range data point could be produced by any reflecting surface patches of an 
object on this spherical surface. We make the assumption that the range return was from 
the beam’s centerline. While it is possible to correct this range once the orientation of the 
feature is known, we do not want to preclude the possibility that a previously unknown 
obstacle of unknown orientation may be producing the return. The errors induced in making 
this assumption will be absorbed by the least squares algorithm to some extent, with the 
remaining error being accounted for in the matching process. 

If a line segment is not successfully matched to a known surface in the 
environmental model, then we must process the segment for further use. Once the 
endpoints are determined, a surface with length equal to the length of the line segment, and 
width proportional to the range from the AUV to the endpoint at the time it was obtained 
(Figure 4-9), is generated. The width is determined as 

= еј = песо ре (Ед 4.31) 
At the selected maximum range of 30 meters (98.25 ft.), d = 2.6m, (7.86 ft.). Choosing 
this width accounts for the fact that the return could have come from any point on the sur- 
face of the spherical section created by the signal. Thus, a 3-D surface is produced from 2- 
D information. The generated poly gon is stored as a set of nodes in the environmental mod- 
el for used in future path planning and post-mission data visualization. At this time, we do 
not provide for the retraction or resizing of surfaces based on subsequently obtained infor- 


mation. 
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transducer 





Figure 4-8. Cross-sectional View of a Sonar’s Beam Pattern. The dis- 
tance d and the error e are proportional to the range. The curvature 
of the spherical surface is exaggerated for illustration purposes. 





Figure 4-9, Projection of Corner Points for 3-D Surface. Corner points 
generated from the line L. The distance dis determined by (Eq 4.31). 
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V. SOFTWARE PROCESSES FOR REAL- 
TIME OBSTACLE AVOIDANCE 


The sonar system installed in the AUV II vehicle plays an integral role in the execution 
of planned missions. The system interfaces with several other processes, as illustrated by 
the current working version of the dataflow diagrams for the AUV software (Figure 5-1 and 
Figure 2-3). The sections below describe the various sonar software processes and the 
interfaces to the other processes seen in Figure 5-1. The interface with the environmental 
database was discussed in Section C of Chapter IV. The actual code associated with the 


sonar processes is found in Appendix B. 
A. SONARSOFTWARE PROCESSES 


The process READ SONAR DATA (5.1) in Figure 5-1 obtains the current range value 
for the appropriate sonar by reading the proper channel of the GESPAC-2b DA/AD 
converter. This raw data is then used to update the moving average in the FILTER DATA 
process (5.2), as described in Section A.6 of Chapter III. The obstacle alert flag will also 
be set here, as described in the next section. The new average range is then used by the 
UPDATE LINE SEGMENT (5.3) process to develop a line segment. If the point is valid 
for the current line segment, then the line parameters are updated and compared to the 
known obstacles by the process MATCH LINE TO MODEL (5.4). If no match is found, 
then the new obstacle flag is set. 

If the new data point causes a termination of the current line segment based on the tests 
described in Section B of Chapter IV, and the new obstacle flag is set, then a new obstacle 
is developed and added to the environmental database as described in Section C.3 of 
Chapter IV by the ADD NEW OBSTACLE process (5.7). If the line segment is terminated, 
then all variables are cleared and a new line will start with the next incoming range data 


point. Once the match process (5.4) is complete, a position update (see Section C.2 of 
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Chapter IV) can be performed if the obstacle was found in the database, i.e. the new _obsta- 


cle flag is not set. 


B. INTERFACE WITH CONTROL GUIDANCE SYSTEM 

There are two occasions when information obtained by the sonar might dictate motion 
control for the AUV. The first instance is the most critical, it is simple obstacle avoidance. 
The second instance arises when it becomes desirable or necessary to map the extent of a 
previously unknown obstacle; this case is discussed in the next section. Sonar range 
information that indicates a potential collision in any direction will be recognized by the 
FILTER DATA process (5.2) in Figure 5-1. 

A minimum safety range for each direction from the AUV (forward, left, right, down) 
is stored as part of the sonar range data structure. As the latest range reading is filtered, the 
minimum safety range 1s compared to the average actual range for that direction. If any of 
the actual ranges fall below the minimum safety range, the obstacle_alert flag bit is set for 
that sonar/direction. This flag is passed to the AVOID OBSTACLES process (8) in Figure 
5-1. 

The AVOID OBSTACLES process determines which course of action to follow if any 
of the flag bits are set. A heading change, a depth change, a full stop, or any combination of 
these changes may be necessary. The various options are shown in Table 5-1. The currently 
installed software generates an “emergency posture” that forces a turn or depth change to 
avoid areas flagged as containing threatening obstacles. While operating in the NPS swim- 
ming pool environment, all turns are ninety degrees. Since the AUV’s dynamics will carry 
it closer to an obstacle during the turn, a logical lockout system prevents the sonar ranges 
from generating any more alerts until the AUV is within 20° of the new heading. If there 
are no known lateral obstacles, an obstacle ahead of the AUV could be avoided by a turn 


to the left or to the right. The default in this case is a nght turn. 
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TABLE 5-1. AUV OBSTACLE AVOIDANCE MANUEVERS 


Obstacle Alert Flag Depth Change 
(Fwd, right, left, bottom) 


a 1 
m [ыш | — 


0 = no obstacle 1 = obstacle x =O or | 





C. MOTION ALGORITHM FOR OBSTACLE MAPPING 

The most reliable method of mapping an underwater feature is to control the motion 
of the vehicle so that the range to the feature remains within the optimum range of the 
sensor, but does not present a collision danger. As described in Chapter IV, the linear 
feature extraction method provides the best resolution at short ranges; therefore, the 
minimum safety range becomes the determining factor in choosing a mapping distance. 

Mapping of the bottom topography with the down-looking sonar is much simpler than 
maintaining a fixed distance laterally from a feature. As noted earlier, the heading of the 
AUV has no effect on the feature extraction of targets below the AUV. Due to the AUV’s 
inherent stability, pitch and roll will affect the bottom sensor only in extreme cases. A 


simple terrain following altitude controller has been implemented and tested in the NPS 


41 


swimming pool. The control law used was 


dels = k (range 


range 


— range) +K, pera9 + Kk 4 (Eq 5.1) 


com 


where dels is the commanded dive plane angle, range ¿o 15 ће desired altitude above the 
bottom, range is the actual sonar range to the bottom, 0 is the pitch angle, and q is the pitch 
rate. 


Mapping of lateral features with the left or right sonars is complicated by the need to 
control heading while attempting to maintain mapping range to the feature. For this reason, 
a simpler scheme involves generating intermediate waypoints and allowing the guidance 
and control processes to maneuver the AUV. Care must be taken to ensure that a large 
lateral step is broken down into smaller steps to ensure that the sonar in contact can 
maintain contact during the maneuver. This means heading changes of no more than five 
degrees relative to the surface of the feature to obtain the highest resolution. The desired 


feature resolution and mission task will ultimately dictate the magnitude of these turns. 
D. INTERFACE WITH ONBOARD MISSION REPLANNER 


At any time that an obstacle is detected, either as a collision danger, or as a previously 
unknown feature, the new obstacle flag is set. This flag, in conjunction with the 
obstacle alert flag, is used to signal the mission executor. One of the roles of the mission 
executor is to determine from these flags if path replanning is necessary, or if obstacle 
mapping may also be required. In order for the mission replanner to accurately assess the 
path to the goal, it must have a complete environmental database. If an emergency turn was 
made due to an obstacle ahead of the AUV, it may be necessary to generate obstacle 
mapping waypoints to determine the extent of the obstacle. This area of research is 


addressed by Wilkinson [Ref. 24], and is a matter of ongoing research. 


ViZEXPERINIEN FAL RESUEPS 


A. EXPERIMENTAL RESULTS OF MODEL MATCHING 

Satisfactory tests of the model matching algorithm discussed in Chapter IV were 
conducted with the AUV operating in the swimming pool. The range data points shown in 
Figure 6-1 are from one such test. Again, the errors previously discussed are manifested in 
an imperfect match to the pool walls. However, by accounting for these known errors in the 
matching, matches between generated line segments and the environmental model are 


possible. 





Figure 6-1. Sample Range Data Set. Range data set used for by the model 
matching algorithm, taken during a test in the NPS swimming pool. Bold 
rectangle represents pool walls. 
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Using an error of 3 ft. in r, and 0.34 radians (20 degrees) in œ, the generated line 


segments in Figure 6-2 represented as dashed lines matched features in the model correctly, 


with no false matches generated. 
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Figure 6-2. Model Matching Results. Line segments fit to the data set 
shown in Figure 6-1, with those segments that were identified by the model 
matching algorithm shown with dashed lines. Numbers represent element 
of model matched, with *s9s” representing no match. Bold rectangle repre- 


sents pool walls. 


REAL-TIME OBSTACLE AVOIDANCE 
The software processes used to perform obstacle avoidance were discussed in Chapter 
V. The results of a pool test mission are seen in Figure 6-3. The original preplanned 


"x", the point corresponding to the 


waypoints for the mission are represented by an 
minimum safety range is marked with an “О”, апа the obstacle avoidance waypoint 


generated by the A VOID OBSTACLES process is shown with a “W”. As demonstrated by 


this test, the sonar processes and gross motion control discussed in Chapter V work as 
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designed, with the AUV safely avoiding an obstacle directly ahead and within the minimum 
safety range. 
C. ALTITUDE CONTROL 

The terrain-following altitude controller discussed in Section C of Chapter V was tested 
inthe NPS pool. Using a filtered signal from the bottom sonar, the controller maintained the 
AUV at a height of 3.0 feet from the pool bottom. As illustrated in Figure 6-4, the controller 


maintained altitude after the initial dive with an error not exceeding 0.5 ft. The tests, using 


the control law from (Eq 5.1), were conducted with k И 259.5 sand 


гапре theta 


k, = 2.5. 


ода ә ғ cx — в 


NPS Swimming Pool 


AUV Track 





Times (secs) —» 


Figure 6-3. Obstacle Avoidance Test. AUV track is shown in upper box, forward so- 
nar range is graphed below. Obstacle within 28 ft. was detected at points marked 
with “O”, resulting in new waypoints (“VV”) to replace original track (“x”). 
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Desired depth (3.0 ft) 


Initial dive 


80 Time (secs) —» 





Figure 6-4. Altitude Controller Performance. AUV depth was commanded to be 3 
ft. and was controlled using the bottom sonar. Pool depth profile during mission is 
shown in lower graph. 
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VII. SIMULATION AND POST-MISSION 3-D DATA MAPPING 


A. AUV MISSION SIMULATOR 
1. Overview 
The role of simulation in a project such as the AUV II project is very beneficial, 
both for testing new processes and for analyzing actual data collected during a test mission. 
Some of the benefits of the current simulator include: 


è Allows software developers to test and debug new modules without the effort 
of a full field test 

e Provides visual display of sonar returns 

e Provides the ability to develop and observe AUV missions before they are 
actually run 

e Provides the ability to visualize the results of a mission from the actual data 
collected during a test. 


The AUV mission simulator is based on the simulator developed by Jurewicz and 
detailed in [Ref. 25]. This simulator, running on a Silicon Graphics IRIS workstation, 
provides a 3-D graphical presentation of the NPS AUV II as it moves through the NPS 
swimming pool, or the Monterey Bay. The simulated AUV is controlled via either mouse 
input through the interface screen, or by manipulating the 6-degree-of-freedom Spaceball 
input device. Velocities, accelerations, and position changes are calculated using the 
control inputs for motor speeds and fin deflections. A detailed hydrodynamic model of the 
AUV is used to compute the simulated motion of the vehicle. 

2. Sonar Simulation 

In order to fully simulate the AUV in the pool environment, a simulation of the 

sonar signals was developed using graphics ray tracing techniques. Each sonar transducer 


is modeled as a point source for a sound ray, with a single ray emanating from the source 
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in the direction of orientation of the sonar. Since the sonars actually emit acoustic energy 
in a cone pattern, six additional rays are simulated to form the outer surfaces of the cone, 


spreading out from the source at a five degree angle (see Figure 7-1). 


End View Side View 





Figure 7-1. Simulated Sonar Beam Using Seven 
Rays. Seventh ray is in center, along sonar axis. 


Each ray 15 traced in a recursive fashion as it intersects the polygons forming the 
pool boundaries. Each point of intersection is stored in an array, and the angle of reflection 
is calculated. Each point then becomes a new source, and the next level of reflection points 
is calculated. This recursion continues until a level of four reflections is reached. If the total 
distance travelled in reaching a point exceeds two times the maximum range of the sonar 
(30 m), then that ray is terminated regardless of the level of recursion, since it would arrive 
back at the sonar after the next ping started. The ray-tracing code is adapted from an 


algorithm found in Glassner [Ref. 26]. 
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After all of the points of intersection are determined and stored, the point found to 
be closest to the sonar, with a reflecting angle within the sonar’s field of view, is 
determined. For a point to be closest, the distance that the ray has travelled to that point 
added to the Euclidean distance from the point to the sonar, must be smaller than for any 
other point. This distance is then divided by two, and a point is projected from the sonar in 
the direction of its orientation, since we assume that all sonar returns come from points 
along the sonar beam’s axis. Any points that were reached by one or more reflections will 
appear farther from the sonar than the actual object (see Figure 7-2). This becomes most 


pronounced when the sonar is directed toward the vertex of a concave corner. 


VIST STS SSS TS SSS SS SSS SSS SS SSS SS ISS SSS] SS ]SS SSS SS SS ]S SSS SSS SSS LS LS SS b: DA 
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False apparent 
range 


Reflected 
echo 


С ХК 





Figure 7-2. False Apparent Range Caused by Re- 
flection. 


B. POST-MISSION REPLAY 

All of the actual test missions run in the NPS pool provide a mission data file that 1s 
downloaded from the AUV at the end of each test run. This information is utilized for 
parameter analysis and sonar performance evaluation. The replay capability of the original 


AUV simulator has also been expanded to accept the data files from actual missions. The 
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data file is loaded onto the graphics workstation where mission replay is selected. Each 
control parameter is used to position the control surfaces, and the positional information is 
used to determine the AUV’s position and orientation for display. 

Additionally, the range values for each sonar are projected from the appropriate sonar 
location for display. As the AUV moves through the mission replay, the track is displayed, 
as well as the actual sonar returns. This capability allows the users to determine if the pre- 
mission simulation was accurate, or if the AUV may have reacted to a situation not foreseen 
by the testers, as evidenced by an unexpected sonar return or track change. The completed 
replay of an actual AUV pool mission is seen in Figure 7-3, with the actual recorded track 
and sonar information labelled. 

The 3-D surfaces discussed in Chapter IV representing unknown obstacles can be 
displayed as well. This capability would have much more significance for a vehicle 
operating in open waters, or in an area where the environmental database was sparse. This 
capability, although limited by the resolution and number of sensors, may well support 


many major missions for AUVs. 
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VIT. CONCLUSIONS AND RECOMMENDATIONS 


A. CONCLUSIONS 


This work addressed four primary objectives, as reiterated here: 


* How can data from ultrasonic sensors be best utilized to recognize obstacles 
during operation of the NPS AUV II? 

* What 1s the optimal configuration for ultrasonic sensors on the AUV II to 
provide obstacle detection and terrain data collection for post-mission 
analysis? 

* What type of motion algorithm will best provide collision avoidance and 
obstacle feature extraction? 

* How can sensor data be utilized in post-mission analysis to generate a 3-D 
terrain model? 


After conducting the development, testing, and evaluation of the various processes 
described in this work, we have reached the following conclusions in response to the 


questions posed in the objectives: 


* The linear feature extraction method presented here, having been initially 
developed on a land vehicle in two dimensions [Ref. 15], proved itself robust 
enough to provide useful information for an AUV operating in three 
dimensions, as well. 

* Given the current availability of four sonar systems, the orthogonal mounting 
arrangement discussed in Chapter II proved satisfactory for obstacle 
avoidance, altitude control, and feature extraction. 

¢ In the swimming pool environment, the processes discussed for obstacle 
avoidance and terrain data collection were demonstrated to be satisfactory. 

¢ The extension of an existing graphical simulation to include a mission replay 
capability provided a satisfactory tool for analysis of 3-D mission data. 


A key factor in the successful development of this work was the availability of a 
readily adaptable graphical simulator. The effort involved in conducting pool tests with the 
AUV, and competing research requirements, precludes frequent testing of individual 
aspects of the vehicle’s systems. Access to a dynamic simulator ensured that basic 


algorithm development and testing could be done independent of the actual vehicle. 
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The goal of classifying obstacles is currently being pursued by others involved in the 
NPS AUV research project. Due to the inherent noise in the sensors and the low resolution 
of the sonars as currently installed, classification of objects must be limited to those objects 
in excess of two feet in size. The calculation of position based on inertial sensing is 


currently being developed and should provide more reliable results in the future, as well. 
B. RECOMMENDATIONS 


The capabilities of NPS AUV II continue to grow as new processes and systems are 
developed and installed. In order to enhance the obstacle avoidance capabilities of the 
vehicle, the following specific recommendations are provided for future work: 


e Provide for the installation of more sonar transducers to provide increased 
coverage of vehicle surroundings. 

e Eliminate the on-board averaging performed by the current sonar processing 
boards. 

e Install an accurate inertial system to provide more accurate positioning data. 

e Consider providing additional processing capabilities to ensure that the 
required control system sampling rates are maintained. 

e Develop simulation capabilities ahead of any new functionality that is installed 
in the vehicle. 


The NPS AUV II project should continue to be a viable research effort for many years, 
and these recommendations are made to ensure that future efforts will be able to build on 


this and other current work. 
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APPENDIX A 


AUV DATA DICTIONAR Y 


Baseline System (v1.1) 


Symbols for Describing Data 
NAME = Description 


Description + Description... (denotes aggregate structure “+’’) 

min{ Description } max (denotes multiple instances “{ }”; if min not specified, assume 
empty; if max not specified, assume infinite) 

«Description | Description» (denotes mandatory "«»" choice "l") 


(Description) (denotes optional item “()””) 


Predefined Data Names 


CHAR = Letters, numbers, space, punctuation 
STRING =0{CHAR }239 

EMPTY = absence of value 

DIGIT= 0.9 

DAY =  l(DIGIT)2 

MONTH =1{DIGIT}2 

YEAR = 4{DIGIT}4 

DATE= DAY + MONTH + YEAR 
INTEGER =1 {DIGIT} 

REAL = {DIGIT} +.+ {DIGIT} 
SECOND=2{ DIGIT}2 

MINUTE = 2{ DIGIT }2 

HOUR = 2{DIGIT}2 

TIME = HOUR + MINUTE + SECOND 


NOTE: BOLD TYPE NAMES REFER TO current Baseline System DFD 


Point 3D = 
X?" T=» REAL 
yau = REALI 
Z ве REAL 


Linear_velocity_3D= 
u = REAL+ 
v = REAL+ 
W REAL 


| ппеашша све ја = 
u_dot= REAL + 
v dot REAL + 
w_dot= REAL 


Attitude_3D= 


phi = REAL + 
theta= REAL + 
pss Web 

Rotational. velocity 3D 
p = REAL+ 
q = REAL+ 
r = REAL 


Rotational_accel_ 3D= 
p_dot= REAL + 
q_dot= REAL + 
rdot= REAL 


Fin_angle = REAL 


Polygon =  3(Point 3D) 

Гоше = 
Position =  Point_3D + 
Velocity =  Linear_velocity_3D + 
Acceleration= Linear _accel_3D + 
Attitude = Attitude 3D+ 


Rotation_vel= Rotational_velocity_3D + 
Rotation_acc=Rotational_accel_3D 
Time = TIME 


Mission_requirement= 


Start point = Point 3D + TIME + 
Intermediate. pointz(Point 3D - TIME] ^ 
Goal = Point_3D + TIME 


Mission_type = INTEGER 
Path = posture) 
Reference_posture =Posture 
Current_posture= Posture 


Replan_request= BOOLEAN 


Range_data= 
Range= REAL + 
Error= REAL 


Sonar data= Range_data + 
Transducer_no=INTEGER 


Track data= Current_posture +4{Sonar_data}4 


Commanded_posture=Posture + 
Mode = INTEGER 


Emergency posture-Posture 
Status = BOOLEAN 


System_status=n [BOOLEAN }п 
Note: n is number of systems reporting 


Obstacle alertZBOOLEAN 
Known_ obstacles= {Polygon} 


New obstacle = 1{Polygon}1 
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Position_update = Point3 + TIME + 
Confidence_factor =INTEGER 


Inertial_data Rotational_velocity_3D + Attitude_3D + 
Depth = REAL + 
Fwd_speed= REAL + 
Mag_hdg= REAL 

Control signal - 

Fin deflectionz$ (Fin, angle] 8 + 
Rt_main_rpm=(DIGIT]3 + 
Lt_main_rpm={DIGIT}3 + 
Fwd_hor_rpm={DIGIT}3 + 
Aft_hor_rpm= { DIGIT}3 + 
Fwd_ver_rpm={ DIGIT}3 + 

Aft ver rpm- (DIGIT)3 


Control positions — Control signal 
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APPENDIX B 


AUV CODE 


ка жж Жж жак ЖЕК ЖЖЖ ЖЖЖ ЖЖ ЖЖ ыы ыыы ы ыыы ыыы ЫЫ ЫЫЫ ЫЫ ы Ыы КЗ 


AUV.H 
Main header file for the auv control code. 


жж жж жож жж о ож зоож ож жооок жо жо И 


#include <stdio.h> 


#ifndef EXTERN 
#define EXTERN extern 
#define INIT(x) 

Hendif 


Hifndef MAIN 
#define EXT extern 
#endif 


#ifdef MAIN 

#define EXT 

#undef MAIN 
Hendif 


Hdefine DAC2B_ADDR OxFFF00040 
#define DAC_LSB_OFFSET 0x2 
#define DAC] ADDR OxFFFOOQOO 


#define ADC1_ADDR (DAC1_ADDR + 0x11) 
#define ADC1_MSB 0x0 

#define ADC1_LSB 0x2 

#define ADC1_CMD_REG 0x4 

#define ADC1_STATUS_REG 0x4 

#define ADC1_BUSY 0x40 


#define ADC2_ADDR OxFFF00020 
#define ADC2_CH_GAIN 0x0 
Hdefine ADC2_STATUS_REG 0x2 
Hdefine ADC2_DATA 0x1 

Hdefine ADC2_CMD_REG 0x2 


Hdefine MFI_BASE OxFFF00700 


Hdefine VIAO_ ADDR OxFFFOOOSO 
#деппе ОКВ КВО 

#define ORA_IRA 1 

#define DDRB 2 

#define DDRA 3 


#define TIC_L4 
#define TIC_H 5 


/* GESDAC-2b addr */ 


/* GESPIA-3A bus addr */ 
/* GESPIA-3A i/o reg B */ 
/* GESPIA-3A i/o reg A */ 
/* Data direction reg */ 
/* Data direction reg */ 
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Hdefine TIL_L6 

Hdefine TIL_H 7 

Hdefine T2C_L 8 

#деппе Т2С Н 9 

#define ACR 11 

#define PCR 12 

#define IFR 13 

#define FRONT_RUD_TOP 0 
#define FRONT_RUD_BOT 1 
Hdefine FRONT_PL_RIGHT 2 
Hdefine FRONT_PL_LEFT 3 
Hdefine REAR_RUD_TOP 4 
Hdefine REAR_RUD_BOT 5 
Hdefine REAR_PL_RIGHT 6 
Hdefine REAR_PL_LEFT 7 


#define RIGHT_MOTOR 0 
#define LEFT_MOTOR 2 
Hdefine SUPPLY 1 


#define RIGHT_MOTOR_RPM 0 
#define LEFT_MOTOR_RPM 1 
Hdefine ROLL_ANGLE_CH 7 
#define PITCH_ANGLE_CH 6 
Hdefine ROLL_RATE_CH 9 
#define PITCH_RATE_CH 8 
#define YAW_RATE_CH 10 


struct MFI_PIA ( 


unsigned short pra; /* port register A / data direction A */ 
unsigned short cra; /* control register A */ 

unsigned short prb; 

unsigned short crb; /* control register B */ 


, 


EXT unsigned char Read, PortA(), Read, PortB(); 
EXT unsigned short Read, PortAB(); 


EXT FILE *outfpl; 

EXT FILE *outfp2; 

EXT int data length; 

EXT int loopent; 

EXT int end_test; 

EXT int wrap_count; 

EXT double t; 

EXT double rpm; 

EXT double main motor. deltal,main motor. delta2; 
EXT int main motor. voltl ; 
EXT int main motor. volt2; 


Эх жж ж Ж АЖ ж ЖА ake ahe 3k a ЖЖ ЖЖЖ ЖЖЖ ЖЖЖ ЖЖЖ ЖЖЖ ЖЖ Ж Ж 


Function declarations 
жжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжж 7 


EXT void user_interface(), control_module(); 
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EXT void loop_data(), zero_data(), control_surface(), record data on(); 
EXT void record data. off(), record_data(); 

EXT void initialize, dacsQ),initialize. adcs(); 

EXT void rudder(),planesQ); 

EXT void main motors offO.aliveO: 

EXT void dac (),dac2b(); 

EXT int adc1(),adc2(); 

EXT double heading(),pitch_angle(),roll_angle(),calc_psiQ; 
EXT double roll_rate_gyro(),yaw_rate_gyro(),pitch_rate_gyro(); 
EXT double depth(); 

EXT double right m rpm(),left m rpm(); 

EXT double get, speed(); 


EXT unsigned short psi bit, old; 
EXT double old angle; 

EXT double dg offset,k psik г; 
EXT double k z,k theta,k q; 
EXT double k speed; 

EXT double ki, speed; 


EXT double time limit; 
EXT int num break: 

EXT unsigned start. dwell; 
EXT int direction; 


EXT double x com[20],y. com[20],z com[20]; 

EXT double psi com offset; 

EXT double psi. com. set; 

EXT double psi. com; 

EXT double delta x y z; 

EXT double x, x init; 

EXT double y, y init; /* init pos input in user interface() */ 
EXT double speed; 


EXT int roll rate. O,pitch. rate. O,yaw. rate. 0; 
EXT int roll O,pitch. 0; 
EXT int z_val0; 


EXT int pointer; 

EXT int speed_array[11]; 

EXT double speed_limit, delta_speed; 
EXT double delta_sum_speed; 


EXT int tick, tick], tick2, curr_tick, mask, count; 
EXT double value; 

EXT long date,ume; 

EXT short day; 

EAT char ans; 


J FRIAR A AIR AA HAHAH HAAR ARH AOR ACK ACK HAR ACK RH ыны ыы 


Sonar related data items/functions/structures 
жжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжж 


#define NEWTYPE(x) (x *)(malloc((unsigned)sizeof(x))) 


ПА А А А AIN АБА БАЛАДЕ А OK OE ORO OR 


Sonar switch addresses for use 
with GESPIA interface 
a] 
Hdefine SONAR_SW1 0x0E 
Hdefine SONAR_SW2 0x0D 
Hdefine SONAR_SW3 0x0B 
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#define SONAR_SW4 0x07 
#define SONAR_TRIG1 0x10 
#define SONAR_TRIG2 0x20 


ЈЕ НА УТО ЈАР ОНИ КРУНУ и ЛИ а о Ви ИЕ 


Constant values for use with 
sonar line fitting and obstacle 


avoidance 

A а еу 
Hdefine CONVERT_TO_FEET 0.02398 /* GESDAC2 units -»ft */ 
sidefinc MIN OBSTACLE RNG 25.0 /* Obs avoidance rng (ft) */ 
8dcfine AVG. PTS 10 /* filter avg window size */ 
#define MAX_RNG_DIFF 15.0 /* filter bandpass sides/bottom*/ 
#define MAX_RNG_DIFF_FWD 25.0 /* filter bandpass-fwd sonar */ 


#дсппе МАХ PTS 1200 

define DEG. TO RAD 0.017453 
define FALSE 0 

#деппе ТКОЕ 1 


/* Constants for line seg package */ 

#define cl 3.0 /* sigma factor */ 

#define c2 .6 /* deviation const */ 

#define c3 .20 /* ellipse thinness ratio */ 
#define MIN_NO_PTS 3 /* least sqrs min */ 

#define MAX_BAD_PTS 3 /* least sqrs bad pt buffer sizc*/ 


#ifndef SONAR 
#define EXT extern 
Hendif 


#ifdef SONAR 
#define EXT 
#undef SONAR 
Hendif 


JS ЗЕЛЕН ЛАКЕ o ORG ek eee ek e E NS RON Y AER 


Variable declarations 
ds y) 
EXT int error, range, sw. cnt, sw. com, bad rng, bad. updates, range. index; 
EXT int b range, bad. pt no, obstacle alert, new. obstacle; 
EXT double rangel; 
EXT double range2; 
EXT double range3; 
EXT double range4; 
EXT double error]; 
EXT double error2; 
EXT double avg rng, range com, k range, max delta r, max delta theta; 
EXT double range. array[10]; 
EXT double bad pt buffer[(MAX BAD PTS][2]; 
EXT double pool[6][2]; 
EXT double max. delta r, max. delta. theta; /* input in user. interface() */ 


ПАО А А О ыыы мам ы ыш ыыы кымы мА ыны ышы ы н ыы е Т> шы ы ы ыы ы у ы ыы 


Function declarations 
o os li a / 
EXT void get_init_avg(), get_avg_mg(), initialize_sonars(Q); 
EXT void ping_sonars(), sonars_on(), set_up_sonars(); 
EXT double atan2(); /* not provided by math.h in current compiler */ 
EXT void init_pool(); 
EXT void match. modcl(): 
EXT void reset_line(); 


EXT void record_line_on(); 
EXT void record_line_off(); 
EXT void line segment init(); 
EXT void compute line. seg(); 
EXT void end line segment); 
EXT void swap init, points); 
EXT void convert. coords(); 


A A OR ee 3924 36 606 35 9 16 26 


Data structures 
ПА ЕАУ АИ NUES Rx er poca ore f 


typedef struct 
( /* Data for each sonar's line segment */ 
double sonar. data pts:MAX PTS][2]; f time */ 
double plane. pts[4][3]; 
double sgmxx, 
sgmyy, 
sgmxy, 
sgmyx, 
sgmx, 
sgmy, 
r Sonar, 
theta, sonar; 
double mux, 
muy, 
muxx, 
muyy, 
mux y, 
delta x, 
delta y, 
sgmx2, 
sgmy2; 
double sgm delta sq, 
sigma, 
d major, 
d minor, 
m major, 
m minor; 
double delta line, 
start pt x, 
start, pt. y, 
end pt x, 
end pt y; 
double line. length; 
int 1 S, 
js. 
n S, 
k s, 
end pt no, 
start pL no, 
bad pt, 
too long, 
too. far. apart, 
course change; 
nt depth. change, 
range pt, 
loc, 
min x, 
max x; 
) LINE SEGMENT, *LINE SEG; 
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typedef struct 
( 


int range_index, 
bad_mg, 
bad_updates, 
switch, 
trigger, 
sonar id; 
doublerange_array[ AVG_PTS], 
range_total, 
avg mg, 
max mg diff, 
min Obs rng, 
raw rmg; 
JRANGE DATA; 


ЈОКО АХЕН ЕЖЕН ЖЕНКЕ Ы 


AUVMAIN.C 


Main control code for the auv. 
Fe NE e а тан 


#define MAIN 


#include <errno.h> 
#include <setsys.h> 
#include <math.h> 

#include “‘auv.h” 


unsigned char*dacl_a = DACI_ADDR; /* 4 Channels of DAC ADA-1 DAC */ 
unsigned char* dac2b_a = DAC2B_ADDR; /* 8 channels of DAC DAC-2B */ 
unsigned char*adcl_a = ADC1_ADDR; /* 16 channels of ADC ADA-1 */ 
unsigned short*adc2_a = ADC2_ADDR; /* 16 channels of ADC ADC-?2 */ 
unsigned short* via0 = VIAO_ADDR; /* PIA addr */ 


void init_vars(); 
void init_clock(); 
void check_clock(); 


main() 


RANGE_DATA *front_sonar,*left_sonar, 
*right_sonar,* bottom_sonar; 
LINE SEGMENT *left line; 


count = 0; 

mask = Ox0000ffff: 
obstacle_alert = 0x0; 
new_obstacle = 0; 


front_sonar = NEWTYPE(RANGE_DATA ); 
left sonar Z NEWTYPE(RANGE DATA); 
rght sonar z NEWTYPE(RANGE DATA); 
bottom sonar 2 NEWTYPE(RANGE DATA); 


left line Z NEWTYPE(LINE SEGMENT); 


init, vars(); 
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loop data(); /* Get z com & x com & y com arrays */ 
user. interface(); 
initialize_dacs(); 
initialize_adcs(); 


printf(* Position AUV for Directional Gyro Offset Measurement\n”); 
printf(* Rate Gyro zero measurement\n”); 

print“ Hit Any Key When Ready”); 

ans = getchar(); 

ans = getchar(); 


set_up_sonars(front_sonar, right_sonar, left_sonar, 

bottom_sonar); /*pass in all four sonars in this order*/ 
sonars_on(front_sonar, left_sonar); 

/* pass in two sonars, must be one from each board-> (front or 

right) followed by (left or bottom) */ 


zero_data(); 
printf(‘“‘Starting\n”); 


psi_bit_old = Read_PortAB(MFI_BASE); 
psi_bit_old &= Ox3FFF; 


alive(10,start_dwell); /* Wag fin every 10 seconds for total 
duration of start_dwell seconds */ 


record data on(); 
record line. on(); 


initialize. sonars(front. sonar, left. sonar); 
iniualize. linc(left. line, 3); 
while (end. test) /* 10Hz control loop */ 


init. clock(); 


get avg rng(front sonar); 

gct avg rng(left sonar); 

update line seg(left. linc); 

control module(front, sonar, right sonar, left sonar, bottom sonar); 


ping sonars(front sonar, left. sonar); 


check clock(); /* timed loop control */ 


main motors. off(); 

record data. off(); 

end line segment(left. linc); 
record line. off; 


)/* end main() */ 


EA ы дамыды ады ыы ыы а ады ыы 


init_vars() 
A А АЛА АД ДА ДЕА ВАЉА АЈ ДАДЕ. 


void init, vars() 


data_length = 0; 
loopent 2 0; 

end test 2 1; 
wrap count - О; 
t = 0.0; 


main_motor_volt1 = 512; 
main_motor_volt2 = 512; 
direction = 1; 

old_angle = 0.0; 
psi_com_offset = 0.0; 


x = 0.0; 
y = 0.0; 
speed = 0.0; 
pointer = 0; 


delta_sum_speed = 0.0; 
)/* end init. globals() */ 


ГАЖ жж жж 2 2225 ж А А А а а Ы 


init_clock() 
fr REESE e E ERR ERRARE 


void init. clock() 


 Sysdate(3,&time, &date, & day, & tick); 
tick1=(tick £ mask); 
)/* end init, clock() */ 


ТИНЧИ tox eK ee EEE К Ж ЖЧ h 


check. clock() 


D TL AM C T A LN etx EL 


void check clock() 


_sysdate(3,&time,&date,& day, &tick); 

tick2=( tick & mask ); 

if (tick2 « tick1) 

tick2 = tick2+100; 

value=abs(tick2-tick 1); 

while (value != 10) 

{ 
if (tick2 < tick1) 
tick2=tick2+100; 
value=abs(tick2-tick 1); 
_sysdate(3,&time,& date, & day, & tick); 
tick2=( tick & mask); 


} 
}/* end check_clock() */ 


A eee ы 


The following sonar code is found in the file asonar.c. 


AUV sonar modules called from main loop of control program or 
from other sonar modules. 
ake ak ake SHE ahe 34e he e akk a ake 34e Ske ahe ake a Ske 2 e ake Ak Se SEak ak ak e k k k ak k k k E E EA ЖКК ЖЧ ШААН 


#define SONAR 


Hinclude “auv.h” 
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а а а а а ер А ааа атта» 


set_up_sonars() 
AAA E БАНОВИ НАКИЋ f 
void set. up. sonars(sonarl, sonar2, sonar3, sonar4) 
RANGE DATA *sonarl, *sonar2, *sonar3, *sonar4 ; 
( 
sonarl-»bad rng - 0; /* front */ 
sonar2->bad_rng = 0; /* right */ 
sonar3->bad_rng = 0; /* left */ 
sonar4-»bad rng z 0; /* bottom */ 


sonar1-»bad, updates = 0; 
sonar2-»bad updates - 0; 
sonar3-»bad updates 2 0; 
sonar4-»bad updates = 0; 


sonarl->switch = SONAR_SW1; 
sonar2->switch = SONAR_SW2; 
sonar3->switch = SONAR_SW3; 
sonar4->switch = SONAR_SW4; 


sonar] ->trigger = ЗОМАК ТКІСІ; 
sonar2-»uigger - ЗОМАК ТКІСІ; 
sonar3->trigger = SONAR_TRIG2; 
sonará ->trigger = SONAR_TRIG2; 


sonari->range_total = 0; 
sonar2->range_total = 0; 
sonar3->range_total = 0; 
sonar4->range_total = 0; 


sonarl-»max rng diff 2 MAX RNG, DIFF FWD; 
sonar2-»max rng. diff Z MAX RNG. DIFF; 
sonar3-»max rng diff МАХ ЕМС ПІҒЕ; 
sonar4-»max rng diff Z MAX RNG, DIFF; 


sonarl-»min obs rng - 28.0; /* values for example only */ 
sonar2-»min obs rng - 8.0; /* values are mission dependent */ 
sonar3-»min obs rng - 8.0; /* units in feet */ 


sonará-»min, obs rng = 3.0; 
)/* end set. up. sonars() */ 


ПО О РАСЕ КАКО ANNA OR Kor 


sonars_on() 
ET А А А А ДАЉ ДАЉА А А АД А С А ЕТО | 


void sonars_on(sonar1, sonar2) 
RANGE DATA *sonarl, *sonar2; 


( 
via0[DDRB] = 0x3F; /* set data direction regs */ 
via0[DDRAJ = 0x00; 


via0( ORB IRB] 2 sonarl-»switch & sonar2->switch; /*turn on sonar */ 
tsleep(1); /* switch debounce time 10ms */ 
)/* end sonars_on() */ 


ee ee eee ee ee Ee eee ee ee 
initialize_sonars() 
ОКИ a жж ЖЖЖ ЖҰ GR GORGE] 


void initialize_sonars(sonar1, sonar2) 
RANGE_DATA *sonarl, *sonar?: 
{ 
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int 1; 
for (i = 0; 1 < 20; ++1) 


via0[ORB_IRB] = (sonarl->switch & sonar2->switch) | 


sonar 1->trigger | sonar2->urigger; /*uigger*/ 
viaO0[ORB_IRB] = sonarl->switch & sonar2->switch; /* clear */ 
tsleep(5); /* wait for max range return 50 ms*/ 


get, init avg(sonarl, sonar2); 
)/*end initialize_sonars() */ 


ЈУ УНЕ БН а Ма Но МУ У СА ЧЕ ЕН ЕЈ МЕН A Ы 


initialize_line() 
A O 
void initialize_line(line, loc) 
LINE_SEGMENT *line; 
int loc; 


line->loc = loc; /*set location id # */ 
reset_linc(linc); (ти Мако [о повео, 


ДА А А А А А К АСКЕ ke joke koc ole oleo 


get init avg() 

МОРИО ИИИ А АДЕ А А ЈА А лао Лана A 
void get_init_avg(sonarl, sonar2) 
RANGE_DATA *sonarl, *sonar2; 


int i; 
for(i = 0; i < AVG_PTS; ++i) 
( 


ping_sonars(sonarl, sonar2); 

tsleep(5); 

sonar1-»range array[i] 2 (float)adc2(2,0); /* board 81 */ 
sonar2->range_array[i] = (float)adc2(3,0); /* board #2 */ 
sonar1-»range. total 42 sonarl-»range array [i1]; 
sonar2-»range. total 4-2 sonar2-»range. array [i]; 


) 


sonarl->avg_rng = sonarl->range_total/AVG_PTS; 
sonar2->avg_rng = sonar2->range_total/AVG_PTS; 
sonarl-»range index = 0; 
sonar2->range_index = 0; 

)/* end get_init_avg() */ 


жж жж жік ж жаі жәке жж ж жж ЖКЖ жі 


ping_sonars() 
ДА ДА АД А А А DADA A E NS a 


void ping_sonars(sonarl, sonar2) 
RANGE_DATA *sonarl, *sonar?; 


| 


via0[ORB_IRB] = (sonarl->switch £ sonar2->switch) | sonar] ->tngger | 
sonar2->trigger; 


viaQ[ORB_IRB] = sonarl->switch & sonar2->switch; /* clear */ 
)/* end ping_sonars() */ 
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Пы 


get avg rng() 
С АД АДА ДАДА А ДА ДА А АДА А А А ДЕ ДАДЕ ДАЈ АЈА 


void get avg rng(sonar) 

RANGE DATA *sonar; 

( 
int 1; 
if((sonar->switch == SONAR_SW1) II (sonar->switch == SONAR_SW2)) 
{ 


sonar->raw_rng = adc2(2,0); 


| 


else 


( 
) 


filter_range_data(sonar); 
)/* end get_avg_rng() */ 


sonar->raw_rng = adc2(3,0); 


A ЈОКО Та: 


filter range data() 
МА А ата АБА А А АЊА АЕ БА а а о а о а РУТЕ / 


void filter_range_data(sonar) 
RANGE DATA *sonar; 
( 


if ((sonar-»raw mg » sonar-»avg rng ) Il 
(fabs(sonar-»raw rng - sonar-»avg rng) «2 sonar-»max rng, diff) || 
(sonar-» bad mg »2 MAX BAD PTS)) 
sonar-»range total 2 sonar-»range total - 

sonar-»range array[sonar-»range index]; 

sonar-»range array[sonar-»range index] 2 sonar-»raw. rng; 
sonar-»range total -- sonar-»raw rng; 
sonar-»avg rng - sonar-»range total/AVG PTS; 
sonar-»range index 2 (sonar-»range index + 1) % AVG PTS; 
if(sonar-»bad rng »2 MAX BAD PTS) 
( 


++sonar->bad_updates; 
) 
if(sonar->bad_updates >= MIN_NO_PTS) 
| 


sonar->bad_rng = 0; 


else 
++sonar->bad_rng; 
анаар * CONVERT_TO_FEET) <= sonar->min_obs_mg) 
е sonar->sonar_num 
case T: 
( 


obstacle alert 2 obstacle alert | 0x8; 
break; 
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case 2: 


Obstacle_alert = obstacle_alert 1 0x4; 
break; 
) 


case 3: 


obstacle_alert = obstacle. alert | 0x2; 
break; 
) 


case 4: 

( 
obstacle_alert = obstacle_alert | Ox1; 
break; 


) 


)/* end filter_range_data() */ 


(яхта аза ажа кек лана ааа ee 


update_line_seg() 

TERRES RELA ERASE SRG A A oso moo dU ККЕ ГЕК 
void update_line_seg(line) 
LINE_SEGMENT *line; 


( 
if(line->range_pt <= MIN_NO_PTS) 
{ 


line. segment init(line); 
if(line-2range. pt » MIN NO PTS) 
( 


line_seg_compute(line); 
if(line->bad_pt >= MAX BAD PTS) 


end line. segment(line); 
reset. line(line); 
swap. init. points(line); 


| 


| РА АН БОА А РРА АРА А А ИУ 


reset_line() 

a A јака а а ee | 
void reset_line(line) 
LINE_SEGMENT *line; 

( 
line->n_s=0; 
line->i_s = 0; 
line->start_pt_no = 0; 
linc->end_pt_no = 0; 
line->range_pt = 0; 
line->sgmx = 0.0; 
line->sgmy = 0.0; 
line->sgmx2 = 0.0; 
line->sgmy2 = 0.0; 
line->sgmxy = 0.0; 
line->sgm_delta_sq = 0.0; 
line->max_x = 0; 
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line->min_x = 0; 


IR A EA IO TOUS ж ЕЕ ЖЕК ЕЕЕ 
line_segment_init() 
Initialize a line segment and its associated variables/flags 
Called f,rom sonar. range() in sonar. bay.c. 
жже жж / 
void line_segment_init(line) 
LINE_SEGMENT *line; 
( 
/* Read in first points to establish iniual line segment */ 
line->line_length = 0.0; 
line->bad_pt = FALSE; 
/* accumulate variables */ 
line->sgmx += line->sonar_data_pts[line->n_s)[0]; 
line->sgmy += line->sonar_data_pts[line->n_s][1]; 
line->sgmx2 += SQR(line->sonar_data_pts[line->n_s][0]); 
line->sgmy2 += SQR(line->sonar_data_pts[line->n_s][1]); 
line->sgmxy += line->sonar_data_pts[line->n_s)[0] * 
line->sonar_data_pts[line->n_s][1]; 
line->end_pt_no = line->n_s; 
if(line->sonar_data_pts[line->n_s][O] < line->sonar_data_pts[line->min_x][0]) 
line->min_x = line->n_s; 
if(line->sonar_data_pts[line->n_s][O] > line->sonar_data_pts[line->max_x][0]) 
line->max_x = line->n_s; 
/* Update the counters */ 


if (line->range_pt == 1) 
line->start_pt_no = line->n_s; 
++line->n_s; 
++line->1_s; /* current line segment point counter */ 
if (line->range_pt >= MIN_NO_PTS)/* use x data points for first segment */ 


/* Calculate first line segment values */ 

line->mux = line->sgmx / line->i_s; 

line->muy = line->sgmy / line->i_s; 

line->muxx = line->sgmx2 - (line->sgmx * line->sgmx) / line->1_s; 
line->muyy = line->sgmy2 - (line->sgmy * line->sgmy) / line->1_s; 
line->muxy = line->sgmxy - (line->sgmx * line->sgmy) / line->i_s; 


line->end_pt_no = line->n_s - 1; 
line->theta_sonar = (atan2((-2.0*line->muxy),(line->muyy-line->muxx))) / 2.0; 


line->r_sonar = line->mux * cos(line->theta_sonar) + line->muy * sin(line->theta_sonar); 
for (line->j_s = 0; line->j_s < MIN_NO_PTS; +4line->j_s) 
{ 


line->k_s = (line->j_s + line->start_pt_no); 
line->sgm_delta_sq += SQR(line->sonar_data_pts[line->k_s)[0] - line->mux) 
* SQRcos(line->theta_sonar)); 
line->sgm_delta_sq += SQR(line->sonar_data_pts[line->k_s][1] - line->muy) 
* SQR(sin(line->theta_sonar)); 
line->sgm_delta_sq += 2.0 * (line->sonar_data_pts[line->k_s)][0] - line->mux) 
* (line->sonar_data_pts[line->k_s][1] - line->muy) 
* cos(line->theta_sonar) * sin(line->theta_sonar); 


| 


ПАЗ АД А АЊА О ЛО ЊЕ АДА ЕЕ ХТЖ 
line_seg_compute() 
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Read in subsequent data points, after a line segment has been 
initialized and more range values are obtained 


жжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжж Í 


line_seg_compute(line) 
LINE SEGMENT *line; 


[ 


/* Calculate test values */ 
line->sigma = line->sgm_delta_sq / line->1_s; 
/* Test new point for linearity fit */ 


line->delta_line = line->sonar_data_pts[line->n_s][0] * cos(line->theta_sonar) 

+ line->sonar_data_pts[line->n_s][1] * sin(line->theta_sonar) 

- line->r_sonar; 
if (((fabs(line->delta_line)) <= (line->sigma * c1)) Il ((fabs(line->delta_line)) <= c2)) 
{ 


line->sgmx += line->sonar_data_pts[line->n_s}[{0]; 

line->sgmy += line->sonar_data_pts[line->n_s}[1]; 

line->sgmx2 += SQR(line->sonar_data_pts[line->n_s]{0]); 

line->sgmy2 += SQR(line->sonar_data_pts{line->n_s][1]); 

line->sgmxy += linc->sonar_data_pts[line->n_s][0] * line->sonar_data_pts[line->n_s][1); 
line->mux = line->sgmx / (line->i_s + 1); 

line->muy = line->sgmy / (line->i_s + 1); 

line->muxx = line->sgmx2 - SQR(line->sginx) / (line->i_s + 1); 

line->muyy = line->sgmy2 - SQR(line->sgmy) / (line->1_s + 1); 

line->muxy = line->sgmxy - (line->sgmx * line->sgmy) / (line->i_s + 1); 


/* calculate ellipse values */ 

line->m_major = (line->muxx + line->muyy) / 2.0 - sqri((line->muyy - line->muxx) 
* (linc->muyy - linc->muxx) / 4.0 + SQR(line->muxy)); 

line->m_minor = (line->muxx + line->muyy) / 2.0 + sqri((line->muyy - line->muxx) 
* (line->muyy - line->muxx) / 4.0 + SQR(ine->muxy)); 

line->d_major = 4.0 * sqrt(fabs(line->m_minor / (line->i_s + 1))); 

line->d_minor = 4.0 * sgri(fabs(line->m_major / (line->1_s + 1))); 


/* Test new point for ellipse line->thinness */ 
if ((line->d_minor / line->d_major) « c3) 


line->end_pt_no = line->n_s; /* update end point */ 
line->bad_pt = 0; /*reset moving bad_pt counter */ 
> 

* update line segment parameters to include new 

* point 

ж 


line->theta_sonar = (atan2((-2.0 * line->mux y), 
(line->muyy - line->muxx))) / 2.0; 
line->r_sonar = line->mux * cos(line->theta_sonar) + line->muy 
* sin(line->theta_sonar); 
line->sgm_delta_sq += 2.0 * (line->sonar_data_pts[line->n_s]{0] - line->mux) 
* (line->sonar_data_pts[line->n_s][1] - line->muy) 
* cos(line->theta_sonar) * sin(line->theta_sonar); 
linc-»sgm, delta sq «2 SQR(line-»sonar, data pts[line-»n, s][1] - line-2muy) 
* SQR(sin(line->theta_sonar)); 
line->sgm_delta_sq += SQR(line->sonar_data_pts[line->n_s][0] - line->mux) 
* SQR(cos(line->theta_sonar)); 
1f(line->sonar_data_pts[line->n_s][0] < line->sonar_data_pts[line->min_x][0])) 
line->min_x = line->n_s; 
if(line->sonar_data_pts{line->n_s][0] > line->sonar_data_pts[line->max_x][0]) 
line->max_x = line->n_s; 


++line->n_s; 
++line->1_ Ss; 


line->delta_x = line->sonar_data_pts[line->start_pi_noJ[0] 

- line->sonar_data_pts[line->end_pt_no][0]; 
line->delta_y = line->sonar_data_pts[line->start_pt_no][1] 

- line-»sonar. data pts[line-»end pt no][l]; 
line->line_length = sqrt(SQR(line->delta_x) + SQR(line->delta_y)); 


else 
bad_pt_buffer[line->bad_pt][0] 
[1] 


bad pt buffer[line-»bad. pt] 
++line->bad_pt; 


line->sonar_data_pts[line->n_s]|[0}; 
line->sonar_data_pts[line->n_s][ 1]; 


== 
— 
— 
— 


else 


bad_pt_buffer[line->bad_pt][0] = line->sonar_data_pts[line->n_s][0]; 
bad_pt_buffer{line->bad_pt][1] = line->sonar_data_pts[line->n_s][1]; 
++line->bad_pt; 


) 


ELE S шы ыыы аы ык ы ыы КЫ Ый Кы ыа ЧЫ Бы ышы ый а ышы асы ы ыбы ы uns 
end_line_segment() 
Wrap up a line segment if bad data pt, course change, depth change, 


or segment max length reached 
ПА А КАО АВА РАЈА A 


void end line, segment(line) 
LINE SEGMENT *line; 
( 

Int 1; 

double line angle; 


/* start new line segment */ 


line->sgmx = 0.0; 
line->sgm y = 0.0; 
line->sgmxy = 0.0; 
line->sgmx2 = 0.0; 
line->sgmy2 = 0.0; 
line->sigma = 0.0; 
line->sgm_delta_sq = 0.0; 


/* close out old segment, convert radius to positive value first */ 
if (line-»r. sonar « 0) 
| 
line->theta_sonar = 180 * DEG_TO_RAD + line->theta_sonar; 
line->r_sonar = -1 * line->r_sonar; 
| 
/* determine start and end points on the computed line segment */ 
line->start_pt_x = line->sonar_data_pts[line->start_pt_no][0); 
line->start_pt_y = line->sonar_data_pts[line->start_pt_no][1]; 
line->end_pt_x = line->sonar_data_pts[line->end_pt_no][0]; 
line->end_pt_y = line->sonar_data_pts|[line->end_pt_no]|[1 ]; 
line->delta_line = line->start_pt_x * cos(line->theta_ sonar) 
+ line->start_pt_y * sin(line->theta_sonar) - fabs(line->r_sonar); 
line->start_pt_x = line->start_pt_x - (line->delta_line * cos(line->theta_sonar)); 
line->start_pt_y = line->start_pt_y - (line->delta_line * sin(line->theta_sonar)); 


line->delta_line = line->end_pt_x * cos(line->theta_sonar) + line->end_pt_y * sin(line->theta_sonar) 


~ 
a) 


- fabs(line->r_sonar); 
line->end_pt_x = line->end_pt_x - (line->delta_line * cos(line->theta_sonar)); 
line->end_pt_y = line->end_pt_y - (line->delta_line * sin(line->theta_sonar)); 
line->delta_x = line->start_pt_x - line->end_pt_x;: 
line->delta_y = line->start_pt_y - line->end_pt_y; 
line->line_length = sqrt(SQR(line->delta_x) + SQR(line->delta_y) 

+ SQR(line->sonar_data_pts[line->start_pt_no][2] 

- line->sonar_data_pts[line->end_pt_no][2])); 


if((line->line_length >= 24.0) && (line->loc != 1)) 


match_model(line); 
if(pool_surface == 9) /* no match */ 


create_new_obstacle(line); 
record_line(line); 
new_obstacle = 1; /* set flag */ 
) 
) 


++line->n_s: 


Га јао а а а а O A ee e r 


create_new_obstacle() 
жж Ао А ж ж жж И 


void create_new_obstacle(line) 
LINE_SEGMENT *line; 


if (line->loc == 4)/* Bottom sonar */ 


( 
line_angle = atan2(line->sonar_data_pts[line->start_pt_no][2] 
- line->sonar_data_pts[line->end_pt_no][2], 
line->sonar_data_pts[line->start_pt_no][0] - line->sonar_data_pts[line->end_pt_no][0}); 
line angle 2 line angle - PIOVER2; 
) else 


( 
line angle = atan2(line->sonar_data_pts[line->start_pt_no][1] 
- line->sonar_data_pts[line->end_pt_no][1], 
sqrt(S QR(ine->sonar_data_pts[line->start_pt_no][Q] 
- line->sonar_data_pts[line->end_pt_noJ[0]) 
+ SQR(line->sonar_data_pts[line->start_pt_no][2] 
- line->sonar_data_pts[line->end_pt_no][2]))); 


if ((line->loc == 4) /* Bottom sonar */ 

{ 
offsetic = line->sonar_data_pts[line->start_pt_no][3] * 0.087155 * cos(line_angle); 
offsetls = line->sonar_data_pts[line->start_pt_no][3] * 0.087155 * sin(line_angle); 
offset2c = line->sonar_data_pts[line->end_pt_no][3] * 0.087155 * cos(line_angle); 
offset2s = line->sonar_data_pts[line->end_pt_no][3] * 0.087155 * sin(line_angle); 

} else 


offsetic = line->sonar_data_pts[line->start_pt_no][3] * 0.087155 * cos(line_angle); 


offsetls = line->sonar_data_pts[line->start_pt_no][3] * 0.087155 * sin(line_angle); 
offset2c = line->sonar_data_pts[line->end_pt_no][3]} * 0.087155 * cos(line_angle); 
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offset2s = line->sonar_data_pts|[line->cnd_pt_no][3] * 0.087155 * sin(line_angle); 
) 
set_plane_ptr(line); 
line->n_s=0; 
line->start_pt_no = 0; 
linc->cnd_pt_no = 0; 


for (1 = 0; 1< 100; +41) 
line->sonar_data_pts(1][3] = 0.0; 


) 


OS 
swap. init, points() 

akk Ak ak ak CII жж ak a a жж жж жж жж жж жж жж жж жж жж жж жж жж Жжжж жж / 
void swap_init_points(linc) 
LINE SEGMENT *linc; 


Int 1; 


line->1_s = 0; 
line->range_pt = 0; 
for(i = 0; 1 < line->bad_pt; +41) 


line->sonar_data_pts[linc->n_s][0] = bad_pt_buffer[1J[0); 
linc-»sonar. data pts(linc-»n, s][1] 2 bad pt buffer[1][1]; 
++line->range_pt; 

line. segment init(linc); 


| 


КОШ A ыы а ы ады ы vom x 


convert_coords() 
A 


void convert. coords(linc) 
LINE SEGMENT *linc; 
| 
line->sonar_data_pts[line->n_s][O] = x + avg rng * cos(psi - 1.57079) 
* CONVERT_TO_FEET; 
line->sonar_data_pts[line->n_s][1] = y + avg_rng * sin(psi - 1.57079) 
* CONVERT_TO_FEET; 
++line->range_pt; 


LL ee ee ee et ee ee eee 
set_plane_ptr() 


Store the plane data points into the array for display. 
AA ARS RA 


set. plane, ptr(linc) 
LINE SEGMENT *linc; 
| 
Int i; 
double line. angle; 
if(line->loc == 4) /* bottom sonar */ 


( 


line->plane_pts[0][0] = linc->sonar_data_pts[line->start_pt_no][0] + offsetl1c; 


75 


line->plane_pts[0][1] = line->sonar_data_pts[line->start_pt_no][1]; 
line->plane_pts[0][2] = line->sonar_data_pts[line->start_pt_no][2] + offsetls; 
line->plane_pts[1][0] = line->sonar_data_pts(line->start_pt_no]J[0] - offsetl1c; 


line->plane_pts[1][1] = line->sonar_data_pts[line->start_pt_no][1]; 
line->plane_pts[1](2] = line->sonar_data_pts[line->start_pt_no][2] - ойзе 1; 
line->plane_pts[2][0] = line->sonar_data_pts[line->end_pt_no][0] + offset2c; 


line-»plane, pts[2][1] 2 line-»sonar, data. pts[line-»end, pt. no][1]; 
line->plane_pts[2][2] = line->sonar_data_pts[line->end_pt_no][2] + offset2s; 
line->plane_pts[3][0] = line->sonar_data_pts[line->end_pt_no][0] - offset2c; 


line->plane_pts[3][1] = line->sonar_ data_pts[line->end_pt_noJ(1]; 
line-» plane, pts[3][2] = line->sonar_data_pts[line->end_pt_noJ(2] - offset2s; 
} else 
{ 

line->plane_pts[0][0] = line->sonar_data_pts[line->start_pt_no][0] 
+ offsetlc * cos(line->theta_ sonar); 

line->plane_pts[0][1] = line->sonar_data_pts[line->start_pt_no][2] 
+ offsetls + offsetlc; 

line->plane_pts[0][{2] = line->sonar_data_pts[line->start_pt_no][1]; 

line->plane_pts[1][0] = line->sonar_data_pts[line->start_pt_no][0] 
+ offsetlc * cos(line->theta_sonar); 

line->plane_pts[1][1] = line->sonar_data_pts[line->start_pt_no][2] 
- offsetls - offsetlc; 

line-»plane. pts[1][2] = line->sonar_data_pts[line->start_pt_no][1]; 

line->plane_pts[2][0] 2 line-2sonar. data pts[line-»end pt no][0] 
* offsetlc * cos(line-»theta, sonar); 

line->plane_pts[2][1] 2» line-»sonar. data pts[line-»end pt no][2] 
+ offset2s + offset2c; 

line->plane_pts[2][2] = line->sonar_data_pts[line->end_pt_no]|[1]; 

line->plane_pts[3][0] = line->sonar_data_pts[line->end_pt_no][0] 
+ offsetlc * cos(line->theta_sonar); 

line-»plane. pts[3][1] 2 line-»sonar. data pts[line-»end pt. no][2] 
- offset2s - offset2c; 

line->plane_pts[3][2] = line->sonar_data_pts[line->end_pt_no][1]; 


| 


ТЭА жж жа аа атта аа ХКК а а ЗЕ 


atan2() Not available in GESPAC math lib 


AA A а а а ва O аси 


double atan2(x1,y) 
double x1,y; 
( 


double x,x1, q, q2; 


int Sign; 

if(y != 0.0) 

| х= 
else 
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| 


if(x1»2 0.) 
return(0.0); 


return(3.141592654); 
) 


x1 = alan(x):; 
Ще <0.) && (y > 0.)) 


xi = x1 + 3.141592654; 
) 


па <0.) && (y < 0.)) 


X1 2 x1 + 3.141592654; 
} 


return(x1): 


ОИУ 


init, pool() 


ЖЖЖЖ КА АККЖ ЧК у 


void init_pool() 
{ 


) 


/* роо [0] =г 
роо | 1][1] = theta 
positions are taken relative to start point of AUV (x_init,y_init)*/ 


pool[0][0] 2 y. init; /* north wall - next to launch pt */ 
pool[0][1] 2 1.57078; 
pool[1][0] = 117.58 - x init; /* deeep end */ 
pool[1][1] 2 0 
R ] = 60.36 - y_init; 

pool[2][1] = 1.57078; 
pool[3][0] = x_init; 
pool[3][1] = 3.141592; /*shallow end */ 
pool[4][0] 2 4.0; /*shallow botom */ 
pool[4][1] 2 1.6388; 
pool[5][0] = 8.0; /*deep end */ 
pool[5][1] = 1.57078; 


A A RR A SS: 
match_model() 

e a) 
void match. model(line) 

LINE SEGMENT *line; 


Int 1; 
double delta r, delta. theta; 


77 


pool_surface = 9; /* default for no match */ 

for(i = 0; 1 < 6; ++1) 

( 
delta r = line->r_sonar - pool[1][0]: 
delta_theta = fabs(line->theta_sonar) - pool[1][1]; 
while(delta_theta > 3.141592) 


delta_theta = delta_theta - 3.141592; 
} 


if ((fabs(delta r) «2 max delta r) && (fabs(delta theta) <= max delta theta)) 
( 


pool_surface = i; 
return; 


) 


| RRR лкк» ЖСК КЖЕ pr ааа E 


record_line_on() 
EARLE ELLE RSLS oto No a Ok OK K O S A O оо et ht) 


void record_line_on() 
if((outfp2 = fopen(“obs.d”, “w”)) == 0); /* open file */ 
{ 
exit(- 1); 


| 


(ТЕК КККК ЖЕКЕЧЕ КЕКЕ КЖЕ ККЕ ee 


record_line() 
Kak ak ak ak ak ak ak ak ak ak ak akak ak ak ak ak akak ak ak akak ak ak ak ak ak 3k k ak 3k ak akak akak ak akk ak КК ЖЖЖЖ 


void record_line(line) 
LINE_SEGMENT *line; 


| ae 
int ijj; 
for(i =0; 1 < 4; ++1) 
forG=0; j < 3; ++)) 
fprinti(outfp2, “JZlf”,line->plane_pts[i][5)); 
ет ІЛЕ) 
) 


До АС ОЉА АБА ЛА А ЛАА А аб аж O 


record line. off() 
Мар ас ааа а ЕК EE ey 
void record line. off() 


fclose(outfp2); 
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APPENDIX C 


ЕРТ тк ааа ee OR EEA ES EE EA AAS ROG 09 6 94 345 3: 


Sonar related code for the AUV II simulator in gra; uv/magrino/sim. 

Following is a portion of the header file AUV.H 

ti 
EXT void draw_sonar_plot(); 

EXT void initialize_sonars(); 

EXT void initialize_virtual_pool(); 

EXT void initialize_pool_obs(); 

EXT double обе Пс, offsetls, offset2c, offset2s; 


/* structure definitions */ 


typedef struct Point3Struct { /* 3D point */ 
double x, y, z; 

} Point3; 

typedef Point3 Vector3; 


typedef struct Matrix4Struct [ /* 4 x 4 matrix */ 
double element[4][4]; 
) Mauix4; 


typedef struct ( 
float forces[6];/* summation of forces & moments */ 
float mminv[6][6]; /* inverse mass matrix */ 
float acc[6];/* udot, vdot, wdot */ 
float vel[6];/* u,v,w,p,q,r */ 
float pos_change[6]; 
float delta t;/* time between updates */ 
float pos[3];/* x,y,z */ 
float roll, pitch, heading; 
double commanded posture[1500][17]; /* designated path for temp use */ 
double waypoint[10][4]; 
double psi_0; 
int wpts; 
int obs. avoid; 
int posture. no; 
Mauix H  matix; 
Matrix incremental H.. matrix; 
Mauix T. matrix: 
) DYNAMIC STRUC, *Dyn pi; 


typedef struct Triangle ( 


Point3 vO; 
Point3 v1; 
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Point3 v2; 
Vector3 normal; 
int 11; 

int 12; 

double d; 
double r; 
double theta; 


) triangle; 
EXT triangle pool[28]: 


typedef struct 


| 


/* Data for each sonar’s line segment */ 
double sonar_data_pts[1500][7];/* x,y,z,raage,course, depth */ 
double plane pts[100]J[4][3]; 
double sgmxx, 
sgmyy, 
sgmxy, 
sgmyx, 
sgmx, 
sgmy, 

r sonar, 

theta sonar; 
double mux, 
muy, 

muxx, 

muyy, 

muxy, 
delta_x, 
delta_y, 
sgmx2, 
sgmy2; 
double sgm , delta sq, 
sigma, 

d major, 

d minor, 

m major, 

m minor; 
double delta line, 
start pi x, 
Start. pL y, 
end pt x, 
end pt y; 
double line. length; 
int 1. S, 

J_S, 

n s, 

end pt no, 
start pL no, 
bad pt, 

too. long, 

toO. far. apart, 
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course_change; 
int depth. change, 
range pt, 

n plane, 

loc; 


TCINE SEGMENT, “LINE SEG; 


typedef struct 


| 


/* Position and orientation of a transducer */ 
Matrix sonar_ matrix; 
Point3 *position; 
Vector3 *direction; 
Vector3 *negate. dir; 
double max range; 
LINE SEGMENT line. data; 


) TRANSDUCER, *SONAR HEAD; 


typedef struct 


| 


Setool al transducers. * / 
int bottom_on; 
int right on; 
int left on; 
int front. on; 
TRANSDUCER bottom. sonar; 
TRANSDUCER right sonar; 
TRANSDUCER left. sonar; 
TRANSDUCER (ront sonar; 
TRANSDUCER top sonar; 


) SONAR , SUITE, *SONAR PTR; 


typedef struct ( 


float bottom. clearance; 

float gravity;/* acceleration due to gravity */ 
float rho;  /* water density of environment */ 
float nu; — /* water viscosity of environment */ 


AU TOPIOTI STROCT autor: 

OBSERVER obs; 

VEHICLE GEOMETRY geo; /* vehicle geometry struct */ 
FLAGS sys; /* flags struct */ 

COEFFICIENTS coeff; /* hydro coefficients struct*/ 
MBARI BAY bay; 

DYNAMIC STRUC dyn;/* ivehicle position struct */ 
AUV POLYGONS poly;/* auv objects (polygons) */ 
SURFACES surf; 

RECORDER rec; 

NPS. POOL pool; 

PANELS panel; 

SEMAPHORES multi; 

CONSTRAINTS constraint; 

SONAR_SUITE SONAR; 
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double front_range; , 


) Submarine, *Sub. ptr; 


ALAS AAA AL EE 


Sonar specific constants/variables found in SONAR.H 
BE EE AS SM ДЕ АДАД АДАД ДА АЕ пана а ИМ ОЛ а 
#include <stdio.h> 

#include <math.h> 

/* Constants for sonar package */ 

#define MAX_LINE_LENGTH 200.0 

#define MAX_PT_GAP 300.0 

#dcfine MAX_COURSE_CHANGE 10.0 

#dcfine MAX_SONAR_RANGE 1100.0 

#define MAX_DEPTH_CHANGE 20.0 

#define MIN_NO_PTS 3 

#define MAX_RAND 37767.0 

#define SQR(x) ((x) * (x)) 

#dcfine CONVERT_TO_INCHES .2879 


/* Constants for linc scg package */ 


#define c1 3.0 /* sigma factor */ 

#define c2 .6 /* deviation const */ 

#define c3 .60 /* ellipse thinness ratio */ 

#define c4 24.0 /* minimum line segment Icngth */ 
typedef struct /* ray-uacing structurc */ 


( 
double ray_matrix[100][8]; 
int ray_matrix_index; 
Vector3 *ray_origin; 
Vector3 *ray_direction; 
JSONAR RAY, *RAY STRUCT; 


o on o ool Ib Ж Ж ЖЖ ЖОКЕ ы 


AUV SIMULATOR SONAR SIMULATOR 


This package is called from main, loop.c of auv each time that the auv position is updated. The procedure 
initialize_sonars() 1s called from the initialize_auv() of initialize.c, it sets all of the sonars to their initial po- 
sitions and orientations. The procedure sonar_range performs a ray-trace routine to determine if a sonar beam 
has intersected with a world polygon in a manner to give a valid sonar return. Sonar, range sends a valid range 
point to the package line_seg_bay.c for least-squares fitting, and determination of a plane through the line that 
has been fit to the data points. The procedure plot, sonar() displays the range points and the planes in the pool.- 
Constants are contained in the sonar.h header file, and structures are in the auv.h file. 


жжжжжжжжж жжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжж жи жжжжжжжжжжжжжжжжжжжжжжжж жжжжжжжжжжжжж / 


#include <math.h> 
#include <stdio.h> 
#include <malloc.h> 


#include <sys/types.h> 


#include “auv.h” 
#include “sonar.h” 


#define NEWTYPE(x) (x *)(malloc((unsigned)sizeof(x))) 
Де tunction declarations *****7/ 


double V3Dot(); 

Vector3 * V3New(); 

Vector3 * V3Negate(); 

Vector3 * V3Duplicate(); 

double V3Length(); 

double V3SquaredLength(); 

Vector3 * V3Normalize(); 

Vector3 * V3MultB yScalar(); 

Vector3 * V3DivideB yScalar(); 

Vector3 * V3Sub(); 

Vector3 * V3Add(); 

double V3DistanceBetweenTw cPoints(): 

void V3MultMatrixB yPoint(); 

void reset line(); 

void check length(); 

void check_distance(); 

void check_course(); 

void check_depth(); 

void ping_sonar(); 

void trace_ray(); 

Vector3 *compute_reflecuonQ; 

void get range(); 

double compute distance. from. sonar(); 

void initialize, ray, struct(); 

void position, sonar(); 

void direct. sonar(); 

void replay. sonar. data); 

int match, model(J; 

ieee AA AE ады ONT 34 315 24 34: 34 

initialize_sonars() 

Set up the individual sonar head directions, locations, and ranges. 

ТАК жж ЗА НЕКА eo He a И 

void initialize_sonars( SONAR_AUY) 

SONAR_SUITE *SONAR_AUV; 

( 
int 1, j,k; 
SONAR_AUV->bottom_on = TRUE; 
SONAR AUV:-»bottom. sonar.direction 2 V3New(0.0, -1.0, 0.0); 
SONAR_AUV->bottom_sonar.position = V3New(0.0, -1.0, 0.0); 
SONAR_AUV->bottom_sonar.line_data.too_long = FALSE; 
SONAR_AUV->bottom_sonar.line_data.too_far_apart = FALSE; 
SONAR_AUV->bottom_sonar.line_data.course_change = FALSE; 


SONAR_AUV->bottom_sonar.line_data.depth_change = FALSE; 


SONAR_AUV->right_on = TRUE; 
SONAR_AUV->right_sonar.direction = V3New(1.0, 0.0, 0.0); 
SONAR_AUV->right_sonar.position = V3New(0.0, -1.0, 0.0); 
SONAR_AUV->right_sonar.line_data.too_long = FALSE; 
SONAR_AUV->right_sonar.Jine_data.too_far_apart = FALSE; 
SONAR_AUV->right_sonar.line_data.course_change = FALSE; 
SONAR_AUV->right_sonar.line_data.depth_change = FALSE; 


SONAR_AUV->left_on = TRUE; 
SONAR_AUV->left_sonar.direction = V3New(-1.0, 0.0, 0.0); 
SONAR_AUV->left_sonar.position = V3New(0.0, -1.0, 0.0); 
SONAR_AUV->left_sonar.line_data.too_long = FALSE; 
SONAR_AUV->left_sonar.line_data.too_far_apart = FALSE; 
SONAR_AUV->left_sonar.line_data.course_change = FALSE; 
SONAR_AUV->left_sonar.line_data.depth_change = FALSE; 


SONAR_AUV->front_on = TRUE; 
SONAR_AUV->front_sonar.direction = V3New(0.0, 0.0, -1.0); 
SONAR_AUV->front_sonar.position = V3New(0.0, -1.0, 0.0); 
SONAR_AUV->front_sonar.line_data.too_long = FALSE; 
SONAR_AUV->front_sonar.line_data.too_far_apart = FALSE; 
SONAR_AUV->front_sonar.line_data.course_change = FALSE; 
SONAR_AUV->front_sonar.line_data.depth_change = FALSE; 


SONAR_AUV->bottom_sonar.negate_dir = V3New(0.0, 1.0, 0.0); 
SONAR_AUV->right_sonar.negate_dir = V3New(-1.0, 0.0, 0.0); 
SONAR_AUV->left_sonar.negate_dir = V3New(1.0, 0.0, 0.0); 
SONAR_AUV->front_sonar.negate_dir = V3New(0.0, 0.0, 1.0); 


SONAR_AUV->bottom_sonar.position = V3New(0.0, 0.0, 0.0); 
SONAR_AUV->right_sonar.position = V3New(0.0, 0.0, 0.0); 
SONAR_AUV->left_sonar.position = V3New(0.0, 0.0, 0.0); 
SONAR_AUV->front_sonar.position = V3New(0.0, 0.0, 0.0); 


SONAR_AUV->bottom_sonar.line_data.loc = 1; 
SONAR_AUV->right_sonar.line_data.loc = 2; 
SONAR_AUV->left_sonar.line_data.loc = 3; 
SONAR_AUV->front_sonar.line_data.loc = 4; 


SONAR_AUV->bottom_sonar.max_range = MAX_SONAR_RANGE; 
SONAR_AUV->right_sonar.max_range = MAX_SONAR_RANGE; 
SONAR_AUV->left_sonar.max_range = MAX_SONAR_RANGE; 
SONAR_AUV->front_sonar.max_range = MAX_SONAR_RANGE; 


Гог (1-0; і< 100; ++1) 
( 
SONAR_AUV->bottom_sonar.line_data.plane_pts[1][0][0] 2 0.0; 
SONAR AUV-»right, sonar.line. data.plane pts[i][0][0] 2 0.0; 
SONAR, AUV-»left, sonar.line. data.plane, pts[1][0][0] 2 0.0; 
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SONAR, AUV-»front sonar.line. data.plane. pts[1][0][0] 2 0.0; 


) 


Jette xar oO ВИ Е ЗОИ ее ж 3 


initialize_ray_struct() 
жжжжжжжжжжжжжжжжжжжжжжжжжжжж жжжжжжжжжжжжжжжжжжжжжжжжж 


void initialize_ray_Struct(ray_struct) 
SONAR_RAY *ray_struct; 
( 


int ij; 


for(i = 0; 1 < 100; ++1) 


{ 
for(j = 0; j < 8; ++)) 
{ 
ray struct-»ray matrix[i][j] 2 0.0; 
) 


) 


ray_struct->ray_matrix_index = 0; 

ray_strucl->ray_origin = V3New(0.0, 0.0, 0.0); 

ray_struct->ray_direction = V3New(0.0, 0.0, 0.0); 
) 


A ++ тк A mM E 


* draw_sonar_plot 


* Select cach sonar in sequence, check for an echo, then plot data. 
ж 


ЗА АВА АД А АДА А ЗА АВА ДА А КОР. 
void draw_sonar_plo(SONAR_AUV, auv) 
SONAR_SUITE *SONAR_AUV; 
Sub_ptr auv; 
( 
SONAR_HEAD which_sonar; 
LINE_SEG line_vars; 
volaule SONAR_RAY ray_data; 
volaule SONAR_RAY *ray_struct; 


ray_Struct = &ray_data; 
which_sonar = £«SONAR_AUV->left_sonar; /* used for mission replay-insert desired sonar*/ 


if (auv->constraint.box ) /* actual mission replay switch */ 
{ 
replay_sonar_data(which_sonar, auv); 
) 
else 
( 


iniualize_ray_struct(ray_struct); 
which_sonar = &SONAR_AUV->bottom_sonar; 
If (SONAR_AUV->bottom_on) 

( 
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) 


ping_sonar(which_sonar, auv, ray_struct); 
++ray_struct->ray_matrix_index; 
which_sonar->direcuon->y = -0.99619; 
which_sonar->direction->x = 0.07547: 
which_sonar->direction->z = 0.04357; 
ping_sonar(which_sonar, auv, ray_struct); 
++ray_struct->ray_matrix_index; 
which_sonar->direction->y = -0.99619; 
which, sonar-»direction-»x = 0.07547; 
which_sonar->direction->z = -0.04357; 
ping. sonar(which, sonar, auv, ray, struct); 
--ray struct-»ray matrix index; 
which, sonar-»direction-»y 2 -0.99619; 
which_sonar->direction->x = 0.0; 
which_sonar->direction->z = -0.08715; 
ping_sonar(which_sonar, auv, ray_struct); 
++ray_struct->ray_matrix_index; 
which_sonar->direction->y = -0.99619; 
which. sonar-»direction-»x = -0.07547; 
which_sonar->direction->z = -0.04357; 
ping. sonar(which, sonar, auv, ray_struct); 
--ray suuct-»ray, matrix index; | 
which, sonar-»direction-»y - -0.99619; 
which, sonar-»direction-»x + -0.07547; 
which. sonar-»direction-»z 2 0.04357; 
ping sonar(which sonar, auv, ray. struct); 
--«ray struct-»ray matrix index; 
which, sonar-»direction-» y 2 -0.99619; 
which. sonar-»direction-»x - 0.0; 
which_sonar->direction->z = 0.08715; 
ping_sonar(which_sonar, auv, ray_struct); 
++ray_struct->ray_matrix_index; 


which_sonar = &SONAR_AUV->right_sonar; 
if (SONAR_AUV->right_on) 


| 


ping_sonar(which_sonar, auv, ray_struct); 
++ray_struct->ray_matrix_index; 
which_sonar->direction->x = 0.99619: 
which sonar-»direction-»y - 0.07547; 
which sonar-»direction-»z = 0.04357; 
ping. sonar(which, sonar, auv, ray. struct); 
--ray struct-»ray matrix index; 

which. sonar-»direction-»x z 0.99619; 
which, sonar-»direction-»y - 0.07547; 
which sonar-»direction-»z = -0.04357; 
ping. sonar(which, sonar, auv, ray, struct); 
*-ray. struct-»ray, matrix, index; 

which. sonar-»direction-»x - 0.99619; 
which, sonar-»direction-»y 2 0.0; 
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) 


which_sonar->direction->z = -0.08715; 
ping_sonar(which_sonar, auv, ray_struct); 
++ray_struct->ray_matrix_index; 
which_sonar->direction->x = 0.99619; 
which_sonar->direction->y = -0.07547; 
which_sonar->direcuon->z = -0.04357; 
ping_sonar(which_sonar, auv, ray_struct); 
++ray_struct->ray_matrix_index; 

which. sonar-»direction-»x = 0.99619; 
which_sonar->direcuon->y = -0.07547; 
which. sonar-»direction-»z 2 0.04357; 
ping. sonar(which sonar, auv, ray struct); 
--4ray struct-»ray, matrix, index; 

which. sonar-»direction-»x 2 0.99619; 
which. sonar-»direction-» y + 0.0; 
which_sonar->direction->z = 0.08715; 
ping_sonar(which_sonar, auv, ray_Struct); 
++ray_Struct->ray_matrix_index: 


which_sonar = «SONAR_AUV->lcft_sonar; 
if (SONAR_AUV->left_on) 


| 


ping_sonar(which_sonar, auv, ray_struct); 
++ray_struct->ray_matrix_index; 
which_sonar->direction->x = -0.99619; 
which_sonar->direcuon->y = 0.07547; 
which_sonar->direcuon->z = 0.04357; 
ping sonar(which sonar, auv, ray_Struct); 
++ray_struct->ray_matrix_index; 
which_sonar->direcuon->x = -0.99619; 
which_sonar->direcuion->y = 0.07547; 
which_sonar->direction->z = -0.04357; 
ping. sonar(which, sonar, auv, ray. struct); 
++ray_struct->ray_matrx_index; 
which_sonar->direcuon->x = -0.99619; 
which_sonar->direction->y = 0.0; 

which sonar-»direction-»z 2 -0.08715; 
ping. sonar(which, sonar, auv, ray, struct); 
--ray struct-»ray matrix index; 
which_sonar->dircction->x = -0.99619: 
which_sonar->direction->y = -0.07547; 
which_sonar->direcuion->z = -0.04357; 
ping_sonar(which_sonar, auv, ray_Struct); 
*4ray struct-»ray matrix. index; 

which sonar-»direction-»x - -0.99619; 
which. sonar-»direction-»y = -0.07547; 
which. sonar-»direction-»z = 0.04357; 
ping sonar(which sonar, auv, ray. struct); 
*-ray struct-»ray. matrix index; 
which_sonar->direcuon->x = -0.99619; 
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| 


which_sonar->direction->y = 0.0; 
which_sonar->direction->z = 0.08715; 
ping_sonar(which_sonar, auv, ray_struct); 
++ray_struct->ray_matrix_index; 


which_sonar = &SONAR_AUV->front_sonar; 
if (SSONAR_AUV->front_on) 


( 


) 


ping_sonar(which_sonar, auv, ray_struct); 
++ray_struct->ray_matnx_index; 
which_sonar->direction->z = -0.99619; 
which_sonar->direction->y = 9.07547; 
which_sonar->direction->x = 0.04357; 
ping_sonar(which_sonar, auv, ray_struct); 
++ray_struct->ray_matnx_index; 
which_sonar->direction->z = -0.99619; 
which_sonar->direction->y = 0.07547; 
which_sonar->direction->x = -0.04357; 
ping_sonar(which_sonar, auv, ray_struct); 
++ray_struct->ray_matrix_index; 
which_sonar->direction->z = -0.99619; 
which_sonar->direction->y = 0.0; 
which_sonar->direction->x = -0.08715; 
ping_sonar(which_sonar, auv, ray_struct); 
++ray_struct->ray_matnx_index; 
which_sonar->direction->z = -0.99619; 
which_sonar->direcuion->y = -0.07547; 
which_sonar->direction->x = -0.04357; 
ping_sonar(which_sonar, auv, ray_struct); 
++ray_struct->ray_matrix_index; 
which_sonar->direction->z = -0.99619; 
which_sonar->direcuion->y = -0.07547; 
which_sonar->direction->x = 0.04357; 
ping_sonar(which_sonar, auv, ray_struct); 
++ray_struct->ray_matrix_index; 
which_sonar->direction->z = -0.99619; 
which, sonar-»direction-»y = 0.0; 
which_sonar->direction->x = 0.08715; 
ping_sonar(which_sonar, auv, ray_struct); 
++ray_struct->ray_matrix_index; 


which_sonar = &SONAR_AUV->bottom_sonar; 
which_sonar->direction->x = 0.0; 
which_sonar->direction->y = -1.0; 
which_sonar->direction->z = 0.0; 
direct_sonar(which_sonar,auv); 

if (SONAR, AUV-»bottom on) 


( 


get_range(which_sonar,auv, ray_struct); 
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| 


which_sonar = &SONAR_AUV->right_sonar; 
which_sonar->direction->x = 1.0; 
which_sonar->direcuon->y = 0.0; 
which_sonar->direction->z = 0.0; 
direct_sonar(which_sonar,auv); 


if (SONAR_AUV->right_on) 


get_range(which_sonar,auv, ray_struct); 


) 


which_sonar = &SONAR_AUV->left_sonar; 
which_sonar->direction->x = -1.0; 
which_sonar->direction->y = 0.0; 
which_sonar->direction->Z = 0.0; 
direct_sonar(which_sonar,auv); 


if (SONAR_AUV->left_on) 
( 


| 


which sonar - &SONAR  AUV-»front sonar; 
which. sonar-»direcuon-»x - 0.0; 

which. sonar-»direction-»y - 0.0; 
which_sonar->direcuon->z = -1.0; 
direct_sonar(which_sonar,auv); 

if (SONAR_AUV->front_on) 

{ 


| 


get range(which sonar,auv, ray struct); 


get range(which sonar,auv, ray. struct); 


free(ray struct-»ray. origin); 
(тес(тау struct-»ray. direction); 


reset_sonars(SONAR_AUV);/* Back to orig posits */ 
| /* end else */ 


а ыы е 85252255 052502027 


Draw range marks and derived planes. Front sonar shows range only. 
тт f 


plot_sonar(line,auv) 
LINE_SEGMENT *line; 


float temp1[3], temp2[3], temp3[3], temp4[3]; 
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int fi; 
/* Plot the range points */ 


for (fi = 0; fi « 1500; fi 2 ++fi) 
( 
if (line->sonar_data_pts[fi][3] != 0.0) 
( 
if ((line->loc == 1) Il (line->loc ==3)) 
cmov(line->sonar_data_pts[fi][O], line->sonar_data_pts[fi][1], 
line->sonar_data_pts[fi}[2]); 
else 


2 
* y & z points swapped by line_seg_bay 
* routines, so swap back 


*/ 


if(line->sonar_data_pts[fi][3] == 1.0) 
| 
RGBcolor(200,0,200); 
cmov(line-»sonar. data pts[fi][0], 
line-»sonar. data pts[fi][1], 
line-»sonar. data pts[f1][2]);] 
else 
{ 
cmov(line-»sonar. data pts[fi][0], 
line-»sonar. data pts[fi][2], 
Ппе->зопаг data pts[fi][1]); 
switch(line->loc) 
( 
case 1: 
RGBoolor(255, 255, 0); 
break; 


case 2: 
RGBcolor(0, 255, 0); 
break; 


case 3: 
RGBcolor(0, 0, 255); 
break; 


case 4: 


RGBcolor(255, 0, 0); 
break; 


) 


charstr(“x”); 
1f(auv->constraint.box) 


| 
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emov(line->sonar_data_pts[11][3], line->sonar_data_pis[11][4], 
line->sonar_data pts[fi][5]); 
сПагви(“.”); 


| 
) 
if(auv->constraint.snake) — /* LOS guidance path simulator selection*/ 
( 
cmov(auv-»dyn.waypoint[auv-»dyn.posture. noJ[1], 
auv->dyn.waypoint(auv->dyn.posture_no][2], 
-auv-»dyn.waypoint[auv-»dyn.posture no][0]); 
charstr( ^W"); 
) 


/* Plot the generated planes, sides in white, bottom in black */ 
for (fi = 0; fi < 100; ++11) 
( 

if (line->plane_pts[fi] [0][O] != 0.0) 

| 


ОПИС 0С == 1) 
RGBcolor(0, 0, 0); 


} else 


( 
RGBcolor(10, 200, 100); 


) 

temp1[0] 2 (float) line-»plane. pts[f1][0][0]; 
temp1[1] 2 (float) linc-»plane. pts[fi][O](1]; 
temp1[2] 2 (float) line-»plane, pts[fi][0][2]; 
temp2[0] z (float) line-»plane. pts[fi][1][0]; 
temp2[1] - (float) line-»plane. pts[f1][1][1 ]; 
temp2[2] - (float) line-»plane. pts[fi][ 1][2]; 
temp3[0] - (float) line-»plane. pts[f1][2][0]; 
temp3[(1] 2 (float) line-2plane pts[fi][2][1]; 
temp3(2] 2 (float) line-»plane pts[fi][2][2]; 
temp4 [0] = (float) line-»plane, pts(f1)[3][0]; 
temp4[1] - (float) line-»2plane. pts(fi](3][ 1]; 
temp4(2] = (float) line->plane_pts[fi][{3][2]; 


bgnpoly gon(): 


v3f(templ); 
v3f(temp2); 
v3f(temp4); 
v3f(temp3); 
endpolygon(); 


| 
| 


ЭЖЖ ЖЖЖ ЖЖЖ ЖЖЖ ЖЖЖ ЖЖЖ eoe ako e oko ak ok 
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select 11() & select_i2() 


Returns the appropriate value for use in the point-in-polygon test. 
ЖЖЖ ЖЖ ЖЖЖ E ACNE ACTO O MENOR NON NES ERA кан ка Т” 


select_11(a, b, k) 


double a, b; 
int k; 
( 
switch (pool[k].i1) 
| 
case 0: 
return (a); 
break; 
case 1: 
return (b); 
break; 
) 


) 


select_i2(a, b, k) 
double a, b; 
int K; 


switch (pool[k].12) 
| 
case 1: 
return (a); 
break; 


case 2: 
return (b); 
break; 


) 


ПДВ АД ДАЉА АДАД ДАДА ДА О А A 


reset. sonars() 


Used to return sonars to original values for next cycle. 
АДА АД А А АД АД ДА ДА У eee eee ne 


reset_sonar(SONAR_AUV) 
SONAR. SUITE *SONAR AUV; 
( 


int ij; 


free(SONAR_AUV->bottom_sonar.direction); 

free (SONAR_AUV->right_sonar.direction); 
free(SONAR_AUV->left_sonar.direction); 
free(SONAR_AUV->front_sonar.direction); 
frec(SONAR_AUV->bottom_sonar.negate_dir); 
free(SONAR_AUV->right_sonar.negate_dir); 
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) 


free(SONAR_AUV->left_sonar.negate_dir); 
free(SONAR_AUV->front_sonar.negate_dir); 
free(SONAR_AUV->bottom_sonar.position); 
frec(SONAR, AUV-»right, sonar.position); 
free(SONAR_AUV->left_sonar.position); 
free(SONAR_AUV->front_sonar.position); 


SONAR_AUV->bottom_sonar.direction = V3New(0.0, -1.0, 0.0); 
SONAR_AUV->bottom_sonar.line_data.too_long = FALSE; 
SONAR_AUV->bottom_sonar.line_data.too_far_apart = FALSE; 
SONAR_AUV->bottom_sonar.line_data.course_change = FALSE; 
SONAR_AUV->bottom_sonar.line_data.depth_change = FALSE; 


SONAR_AUV->right_sonar.direction = V3New(1.0, 0.0, 0.0); 
SONAR_AUV->right_sonar.line_data.too_long = FALSE; 
SONAR_AUV->right_sonar.line_data.too_far_apart = FALSE; 
SONAR_AUV->right_sonar.linc_data.course_change = FALSE; 
SONAR_AUV->right_sonar.line_data.depth_change = FALSE; 


SONAR_AUV->left_sonar.direction = V3New(-1.0, 0.0, 0.0); 
SONAR_AUV->left_sonar.line_data.too_long = FALSE; 
SONAR_AUV->left_sonar.line_data.too_far_apart = FALSE; 
SONAR_AUV->left_sonar.line_data.course_change = FALSE; 
SONAR_AUV->left_sonar.line_data.depth_change = FALSE; 


SONAR_AUV->front_sonar.direction = V3New(0.0, 0.0, -1.0); 
SONAR_AUV->front_sonar.line_data.too_long = FALSE; 
SONAR_AUV->front_sonar.line_data.too_far_apart = FALSE; 
SONAR_AUV->front_sonar.line_data.course_change = FALSE; 
SONAR_AUV->front_sonar.line_data.depth_change = FALSE; 


SONAR_AUV->bottom_sonar.negate_dir = V3New(0.0, 1.0, 0.0); 
SONAR_AUV->right_sonar.negate_dir = V3New(-1.0, 0.0, 0.0); 
SONAR_AUV->left_sonar.negate_dir = V3New(1.0, 0.0, 0.0); 
SONAR_AUV->front_sonar.negate_dir = V3New(0.0, 0.0, 1.0); 
SONAR_AUV->bottom_sonar.position = V3New(0.0, 1.0, 0.0); 
SONAR_AUV->right_sonar.position = V3New(-1.0, 0.0, 0.0); 
SONAR_AUV->left_sonar.position = V3New(1.0, 0.0, 0.0); 
SONAR_AUV->front_sonar.position = V3New(0.0, 0.0, 1.0); 


ae eT EER ELE Ee ТУИ итии k 


position_sonar() 
Using the auv H_matrix, move the sonars to the nose. 


ak ak ak ak ak ak ak k ak Ak ak Ak k ak k ak ak ak ak k ak ak ak ak ak ak k ak k ak ak ak k k ak ak k k ak ak ak ak ak k k k k k k k k k / 


void position, sonar(sonar, auv) 
TRANSDUCER *sonar; 
Submarine *auv; 


| 


Vectors “(inne pt: 
Int i, J; 


pushmatrix(); 
loadmatrix(auv->dyn. H_matrx); 


translate(0.0, 0.0, -32.0); 
getmatrix(sonar->sonar_matrix); 
popmatrix (); 


sonar->position->x = sonar->sonar_matrix[3][0); 
sonar->position->y = sonar->sonar_matrix[3][1]; 
sonar->position->z = sonar->sonar_matrix[3][2); 


[EEE EEE EE EEE TE EE EE O P E 


Orient the sonars depending on heading, pitch, roll. 
di жЕ Жа Жк ККК ЖК ЖОК ЖОК ЖОЕ СЕ КЕНСЕ К ОК ee, 


void direct_sonar(sonar, auv) 
TRANSDUCER * sonar; 
Sub_ptr auv; 


double temp; 
double alpha = auv->dyn. heading * DEG. TO RAD; 


pushmatrix(); 
loadmatrix(auv-»dyn.H, matrix); 
getmatrix(sonar-»sonar. matrix); 
popmatrix(); 


switch (sonar-»line data.loc) 


( 


case Ï: 


V3MultMatrixByPoint(sonar); 

sonar->direction = V 3Normalize(sonar->direction); 
sonar->negate_dir->x = -sonar->direction->x; 
sonar->negate_dir->y = -sonar->direction->y; 
sonar->negate_dir->z = -sonar->direction->z; 
break; 


case 2: 


V3MultMatrixByPoint(sonar); 

sonar->direction = V 3Normalize(sonar->direction); 
sonar->direction->z = -sonar->direction->z; 
sOnar->negate_dir->x = -sonar->direction->x; 
sonar->negate_dir->y = -sonar->direction->y; 
sonar->negate_dir->z = -sonar->direction->z; 
break; 


case 3: 
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V3MultMatrixB yPoint(sonar); 

sonar-»direction 2 V3Normalize(sonar-»direction); 
sonar-»direction-2z - -sonar-»direction-»z; 
sonar->negate_dir->x = -sonar->direction->x; 
sOnar->negate_dir->y = -sonar->direction->y; 
sonar->negate_dir->z = -sonar->direcuion->z; 
break; 


case 4: 


V3MultMatrixByPoint(sonar); 

sonar-»direction 2 V3Normalize(sonar-»direction); 
sonar->negate_dir->x = -sonar->direction->x; 
sonar->negale_dir->z = -sonar->direcuion->z; 
sonar->negate_dir->y = -sonar->direction->y; 
break; 


case 5: 


sonar->direclion->x = sin(auv->dyn.pitch * DEG_TO_RAD) * cos(alpha); 
sonar->direction->y = (fabs(cos(auv->dyn.pitch * DEG_TO_RAD))); 
sonar->direction->z = -(sin(auv->dyn.pitch * DEG_TO_RAD) * sin(alpha)); 
sonar->direction = V3Normalize(sonar->direction); 


sonar-»negate dir-»x = -sonar-»direction-»x; 
sonar->negate_dir->y = -sonar-»direction-» y; 
sonar-»negate dir-»z - -sonar-»directüon-»2z; 


ПАЗ А А А А АДА АД АДА АС О ДС АЉАСКА 


reset_line() 


У А АДА ы Ды ым ыы ыды ылы И ту 


void reset_line(line, closest_pt, min_range, auv) 
LINE_SEGMENT *line; 

Point3 *closest_pt; 

double min_range; 

Sub ptr auv; 


| 


| 


line->n_s = 0; 
line->i_s = 0; 
line->start_pt_no = 0; 
line->end_pt_no = 0; 
line->range_pt = 0; 
line->sonar_data pts[0][0 
line->sonar_data_pts[0] 
line->sonar_data_pts[0] 
] 
| 
| 


| closest_pt->x; 

[ 

[ 
line-»sonar, data. pts[O][ 

[ 

[ 


closest. pt-»y; 

closest. pt-2z; 

min range; 
auv-»dyn.heading; 
auv->dyn.H_matrix[3][1); 


1 
012 
3 


line->sonar_data_pis[0][4 


] 
] 
) 
| 
] 
line->sonar_data_pts[0][5] = 


[OGIO жк жж Жжжж ж жж Жжж Жж жж жож жж кж жж» 
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check length() 


ХЕ ЖЖ ЖЖЖ Ж ЖЕЖ ж жж ааа E RR) 


void check_length(line) 
LINE_SEGMENT *line; 


( 


) 
/ 


line->line_length = fsqr(SQR(line->sonar_data_pts[line->start_pt_no][0] - 
line->sonar_data_pts[line->end_pt_no][0]) + 
SQR(line->sonar_data_pts[line->start_pt_no][1] - 
line->sonar_data_pts[line->end_pt_no][1]) + 
SQR(ine->sonar_data_pts[line->start_pt_no][2] - 
line->sonar_data_pts[line->end_pt_no][2])); 
if(line->line_length > MAX_LINE_LENGTH) 
line->too_long = TRUE; 


жжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжж 


check_distance() 


SAEED Oe тт жая жа ее 


void check_distance(line, closest_pt) 
LINE_SEGMENT *line; 
Point3 *closest_pt; 


| 


/ 


double distance_between_pts; 


if (line->loc == 1) 
distance_between_pts = fsqrt(SQR(line->sonar_data_pts[line->end_pt_no][0] - 
closest_pt->x) + SQR(line->sonar_data_pts[line->end_pt_no][1] - 
closest_pt->y) + SQR(line->sonar_data_pts[line->end_pt_no][2] - 
closest_pt->z)); 
else 
distance_between_pts = fsqrt(SQR(line->sonar_data_pts[line->end_pt_no][0] - 
closest_pt->x) + SQR(line->sonar_data_pts[line->end_pt_noJ[1] - 
closest_pt->z) + SQR(line->sonar_data_pts[line->end_pt_no][2] - 
closest_pt->y )); 


if (distance_between_pts > MAX_PT_GAP) 
( 


) 


line->too_far_apart = TRUE; 


) 


жжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжжж 


check_course() 


сада жЕ ЕК ЕК ЖЖ ЕЕЕ ЖЖ oe Ии 


void check_course(line, auv) 
LINE SEGMENT *line; 
Sub ptr auv; 


| 


double heading_difference; 
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heading_difference = line->sonar_data_pts[line->start_pt_noJ[4] - auv->dyn. heading; 
if (heading. difference « 0.0) 
heading difference 2 heading. difference + 360.0; 
if (heading, difference » 180.0) 
heading. difference = 360.0 - heading. difference; 


If (heading. difference » MAX COURSE CHANGE) 
line-2course change = TRUE; 


A e 


check_depth() 
Б аа аа ааа ааа асад, 
void check depth(line, auv) 
LINE SEGMENT *line; 
Sub pir auv; 
{ 
if(fabs(ine->sonar_data_pts[line->start_pt_no][5] - auv->dyn.H_matrix[3][1]) > 
MAX_DEPTH_CHANGE) 
line->depth_change = TRUE; 


ЗА АЖЖ ДА АДА ДА ok kc ЖЖЖ ЖЖЖ ЖЖЖ ЖЖЖ 


ping sonar() 
LLL e Eos NUR УНУ Р УОИ Ееее теу 

void ping_sonar(sonar, auv, ray_struct) 

TRANSDUCER *sonar; 

Sub_ ptr auv; 

SONAR_RAY *ray_struct; 

| 
direct_sonar(sonar, auv); 
position_sonar(sonar,auv); 
ray_Struct->ray_direction->x = sonar->direction->x; 
ray_Struct->ray_direction->y = sonar->direction->y; 
ray_struct->ray_direction->z = sonar->direction->z; 
ray_struct->ray_origin->x = sonar->position->x; 
ray_struct->ray_origin->y = sonar->position->y; 
ray_Struct->ray_origin->z = sonar->position->z; 
trace_ray(ray_struct, sonar); 


EL А А ДА А А ЛА АД О АДА АЈ А САО А А А А А АЊА 


trace_ray() 
РОУ А А ЕД А ы аза АЗ а кыы КЫ ЖАС 
void trace_ray(ray_struct, sonar) 
SONAR_RAY *ray struct; 
TRANSDUCER *sonar; 
Vector3 *pool normal, “тау negate. direction, *R; 
int inter, any. inter, 1, g. s, index, node. index; 
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double d_sonar, t, ND, NO, U0, U1, U2, VO, V1, V2; 
double alpha, beta, distance, return_angle, cum_range, shortest_distance; 
Рош3 *P_inter, 

Point3 *closest pt; 

Point3 *temp рі; 

P inter 2 V3New(0.0, 0.0, 0.0); 

closest pt 2 V3New(0.0, 0.0, 0.0); 

temp. pt 2 V3New(0.0, 0.0, 0.0); 

inter = FALSE; 

any_inter = FALSE; 

ray_negate_direction = V3New(0.0,0.0,0.0); 

R = V3New(0.0,0.0,0.0); 

ray negate direcuon-»x - -ray. struct-»rzy. direction-»x; 
ray, negate directon-»y - -ray_struct->ray_direction->y; 
ray negate direction-»z - -ray_struct->ray_direction->z; 
index = ray struct-»ray matrix index; 

cum, range z 0.0; 

shortest distance - 9999.0; 


/* Cycle through all pool polygons in initialize. virtual  pool.c */ 
for (1 = 0; i < 28; ++1) 
( 
/* Check for proper reflection angle (> O degrees) */ 
return_angle = V3Dot(&pool[i].normal, ray negate direction); 
if (return_angle > 0.0) 
{ 
ND = V3Dot(ray_struct->ray_direction, &pool[i].normal); 
NO = V3Dot(ray_struct->ray_origin, &pool[i].normal); 
temp_pt->x = -pool(1].v0.x; 
temp_pt->y = -pool[i].v0.y; 
temp_pt->z = -pool(1].v0.z; 
d_sonar = V3Dot(temp_pt, &pool[i].normal); 
t= -1 * ((d_sonar + NO) / ND);/* Quick range check */ 
И (С > 0.0) && (t<=2 * MAX_SONAR_RANGE) && (t < shortest_distance)) 
( 
/* Calculate the point of intersection */ 
P_inter->x = ray_struct->ray_origin->x + ray_struct->ray_direction->x * t; 
P_inter->y = ray_struct->ray_origin->y + ray_struct->ray_direction->y * t; 
P_inter->z = ray_strucl->ray_origin->z + ray_struct->ray_direction->z * t; 


UO = select_11(P_inter->x, P_inter->y, 1) - 
select. 1l (pool(1]. vO.x, pool(i].vO.y, 1); 

VO = select. i2(P. inter-»y, P_inter->z, 1) - 
select. i2(pool[1]. vO. y, pool[i]. vO.z, i); 


inter = FALSE; 


Ul = select_11(pool[1].v1.x, pool[1].v1.y, 1) - 
select, il (pool(i].vO.x, pool[i].vO.y, i); 
U2 = select. il(pool(i].v2.x, pool[i].v2.y, 1) - 
select il (pool(1]. vO.x, pool(1].vO.y, 1); 
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V] = seleci_12(pool[1].v1.y, pool[1].v1.z, 1) - 
select_12(pool[i].v0.y, pool[i].vO.z, 1); 
V2 = select_i2(pool[i].v2.y, pool[1].v2.z, 1) - 
select. i2(pool[1]. vO.y, pool[1].v0.z, 1); 


И 
* Check to see if point on plane is within 
* polygon 
2 
о) 
( 
beta = U0 / U2; 
if ((beta >= 0.0) && (beta <= 1.0)) 
{ 
alpha = (VO - beta * V2)/ V1; 
inter = ((alpha >= 0.0) & & (alpha + beta <= 1.0)); 


else 


БЕСЕ ео ОЕ ОО У КОИОТ); 
il ((beta >= 00) && (Беш <= 1.0)) 
{ 
alpha = (U0 - beta * U2) / UT: 
inter = ((alpha >= 0.0) && (alpha + beta <= 1.0)); 


| 


if (inter) /* Point was in poly */ 

| 

any_inter = TRUE; 

distance = V3DistanceBetweenTwoPoints(P_inter,ray_struct->ray_origin); 
if (distance < shortest_distance) 


( 
shortest_distance = distance; 
closest_pt->x = P_inter->x; 
closest_pt->y = P_inter->y; 
closest_pt->z = P_inter->z; 
node_index =1; 

) 


) 


if (any_inter) 
{ 
К - compute reflection(ray. struct, node. index); 
ray suuct-»ray matrix[index][0] 42 shortest. distance; 
cum range - ray struct-»ray matrix [index][0]; 
ray struct-»ray matrix[index][1] 2 closest. pt-»x: 
ray struct-»ray matrix [index ][2] = closest. pt-»y; 
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ray_struct->ray_matrix[index][3] 2» closest. pt-»z; 
ray struct-»ray matrix[index][4] 2 R-»x; 

ray. struct-»ray, matrix[index][5] 2 R-»y; 

ray struct-»ray matrix[index][6] 2 R-»z; 

«*-4ray struct-»ray. matrix [index ][7] ; 


if (ray. struct-»ray. matrix[index][(0] < 2 * MAX SONAR, RANGE) 


[* 

* reset ray 

* origin/direction-recurse 

7) 

( 
++ray_struct->ray_matrix_index; 
ray_struct->ray_matrix_index = ray_struct->ray_matrix_index % 100; 
index = ray_struct->ray_matrix_index; 
ray_struct->ray_matrix[index][0] = cum_range,; 
ray_struct->ray_direction->x = R->x; 
ray_struct->ray_direction->y = R->y; 
ray_struct->ray_direction->z = R->z; 
ray struct-»ray origin-»x - closest, pt-»x; 
ray struct-»ray origin-»y = closest. pt-»y; 
ray_struct->ray_origin->z = closest. pt-»z; 
ray struct-»ray matrix[index][7] += ray_struct->ray_matrix[index-1][7]; 
if (ray struct-»ray. matrix [index][7] « 4.0) /*recursion depth*/ 
trace_ray(ray_struct, sonar); 
| 
) 

free(closest_pt); 

free(P inter): 

free(temp pt); 

free(ray negate direction); 


| 


ОХ Жажа ДА ДА А ee ы 


*compute_reflection() 
| 
Vector3 *compute_reflecuon(ray_struct,i) 
SONAR_RAY *ray_struct; 
int 1; 
( 
Vector3 *L, *two_N, *R, *temp; 
double LdotN; 


L= V3New(0.0, 0.0. 0.0); 

temp = V3New(0.0, 0.0, 0.0); 

R = V3New(0.0, 0.0, 0.0); 

L = V3Duplicate(ray_struct->ray_direcuion); 
L = V3Negate(L); 

LdotN = V3Dot(L, &pool[i].normal); 

L = V3DivideByScalar(L, LdotN); 
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| 


temp 2 V3Duplicate(& pool[1].normal); 

temp = V3MultByScalar(temp, 2.0); 

temp = V3Sub(temp, L, temp); 

R = V3DivideByScalar( temp, V3Length(temp)); 
free(L); 

free(temp); 

return(R ); 


зак иж ake ak ak ЖЖЖ жж аж жж жж 


get гапре() 
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void get_range(sonar,auv, ray_struct) 
TRANSDUCER *sonar; 

Sub_ptr auv; 

SONAR_RAY *ray_struct; 


( 


double min range, range to pt, angle between rays, d, range of closest pt; 
int i, J, min. found; 

Vector3 *ray_negate_direction; 

LINE_SEGMENT *sonar_linc; 

Point3 *closest_pt; 


closest_pt = V3New(0.0, 0.0, 0.0); 
range_of_closest_pt = 9999.0; 

min_range = 9999.0; 

min_found = FALSE; 

ray negate direction 2 V3New(0.0,0.0,0.0); 
sonar line 2 &sonar-»line data; 


for(120; i « 100; 441) 
( 
¡f(ray_struct->ray_matrix[1][7] >= 1.0) 
( 
range_to_pt = V3DistanceBetweenTwoPoints(&ray_struct->ray_matix[i][1], 
sonar-> position); 
if((range to pt 4 ray. struct-»ray. matrix[1][0]) « 2* MAX SONAR RANGE) 
( 
min_range = (range_to_pt + ray struct-»ray matrix[1][0])/2; 
1f ( min_range < range_of_closest_pt ) 


| 


ray_negate_direction->x = ray_struct->ray_matrix[1][1] - sonar->position->x; 
ray_negate_direction->y = ray_struct->ray_matrix[1][2] - sonar->position->y; 
ray_negate_direction->z = ray_struct->ray_matrix[1][3] - sonar->position->z; 
angle_between_rays = V3Dot(ray_negate_direction, sonar->direction): 


if (angle_between_rays > 0.99619) /* cos of five degrees */ 

( 
sonar->line_data.sonar_data_pts[sonar->line_data.n_s}][3] = min_range; 
sonar->linc_data.sonar_data_pts[sonar->linc_data.n_s][6] = min_range: 
min_found = TRUE; 
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range_of_closest_pt = min_range; 

closest_pt->x = ray_struct->ray_matrix(1][1); 
closest_pt->y = ray_struct->ray_matrix[1](2); 
closest pt-»z - ray, struct-»ray, matrix[1][3]; 


| 


if (min. found) 
( 
pushmatrix(); 
loadmatrix(auv-»dyn.H, mauix); 
switch (sonar->line_data.loc) 


| 


case 1: 
translate(0.0, -sonar->line_data.sonar_data_pts[sonar->line_data.n_s][3], 


-32.0); 
break; 


case 2: 
translate(sonar->line_data.sonar_data_pts[sonar->line_data.n_s][3],0.0, 


-32.0 ); 
break; 


case 3: 
translate(-sonar->line_data.sonar_data_pts[sonar->line_data.n_s)](3],0.0, 


-32.0 ); 
break; 


case 4: 
translate(0.0, 0.0, 
-sonar-»line, data.sonar, data pts[sonar-»line, data.n, sJ[3] - 32.0 ); 
auv-»front range - 
sonar->line_data.sonar_data_pts[sonar->line_data.n_s](3]; 
break; 
| 


getmatrix(sonar->sonar_matrix); 
popmatrix(); 


if(match_model(sonar_line) == TRUE) 


( 


sonar->line_data.sonar_data_pts[sonar->line_data.n_s][3] = 1.0; 
++sonar_line->n_s; 
sonar_line->n_s = sonar_line->n_s % 100; 


else 


if(sonar_line->range_pt < 2) 


( 


) 


) 


++sonar_line->n_s; 
sonar_line->n_s = sonar_line->n_s % 100; 


++sonar_line->range_pl; 


sonar->line_data.sonar_data_pts[sonar->line_data.n_s][0] = sonar->sonar_mauix[3][0]; 
sonar->line_data.sonar_data_pts[sonar->line_data.n_s][1] = зопаг->5опаг_ташх[3][1]; 
sonar->line_data.sonar_data_pts[sonar->line_data.n_s][2] = sonar->sonar_matrix[3][2]; 
sonar->line_data.sonar_data_pts[sonar->line_data.n_s][4] = auv->dyn.heading; 
sonar->line_data.sonar_data_pts[sonar->line_data.n_s][5] = auv->dyn.H_matrix[3][1]; 


if (sonar_line->range_pt > 1) 


| 


else 


if(sonar->line_data.sonar_data_pts[sonar->line_data.n_s][3] == 1.0) 


| 


if(sonar_line->range_pt > MIN NO PTS) 


| 


else 


else 


end_line_segment(sonar_line); 
reset_line(sonar_line,closest_pl, min_range, auv): 
sonar_line->sonar_data_pts[sonar_line->n_s][3] = min_range; 


reset_line(sonar_line, closest_pt, min_rangc, auv); 
sonar_line->sonar_data_pts[sonar_line->n_s]}[3] = min_range; 


check_course(sonar_line, auv); 
check_depth(sonar_line, auv); 


if((sonar_line->too_long == TRUE) Il (sonar_line->course_change == TRUE) 
ll (Sonar_line->depth_change == TRUE) Il (sonar_line->too_far_apart == TRUE)) 


| 


if{(sonar_line->range_pt > MIN_NO_PTS) 


| 


else 


end_linc_segment(sonar_line); 
reset_line(sonar_linc,closest_pt, min_range, auv); 
sonar_line->sonar_data_pts[sonar_line->n_s][3] = min_range; 
linc. segment init(sonar); 


reset line(sonar linc, closest. pt, min, range, auv); 
sonar line-»sonar. data. pts[sonar. linc-»n. s][3] 2 min rango; 
line. segment init(sonar); 


( 
++sonar_line->range_pt; 

if (sonar_line->loc != 5) 
if (sonar. line-»range. pt «» MIN NO PTS) 
( 


) 
if (sonar_line->range_pt > MIN_NO_PTS) 
line_seg_compute(sonar); 


line_segment_init(sonar); 


) 
) 
free(ray_negate_direction); 
free(closest_pt); 


) 


РЕКЕ ЖЖЖ Еж ТА ЕЕ а Ы 


match_model() 
BE A A оса а ЖЕ Жа ж Еа ааа аа” 
int match_model(sonar_line) 
LINE. SEGMENT *sonar line; 
( 
int match, 1; 
match = 0; 


for(i = 0; i < 28; ++i) 
( 
1f((fabs(sonar_line->r_sonar - pool[1].r) <= MAX_DELTA_R) & & 
(fabs(sonar_line->theta_sonar - pool[i].theta) <= MAX_DELTA_THETA )) 
( 


) 


match= Ï; 


) 


return(match); 


) 


i 9526: oko sooo oe ooo ЕЖЕ ЖЕКЕ ЕКЕ ЕЕЕ КЕ 


replay_sonar() 
ЖАЖА ЖЕК ККЕ тк а а аа ы 
void replay_sonar_data(sonar, auv) 
TRANSDUCER *sonar; 
Sub ptr auv; 
| 
double SIN 5,raw rng: 
SIN 5 = .087155; 
raw rng - auv->dyn.commanded_posture[auv->dyn.posture_no][14]; 
pushmatrix(); 
Тоадташх(апу->дуп.Н ташх); 
rotate(-(Angle)auv->dyn.pos_change[S], ‘y’); 
rotate( (Angle)auv->dyn.pos_change[4], ‘x’); 
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| 


rotate(-(Angle)auv-»dyn.pos change[3], 'z') 


switch (sonar->line_data.loc) 


| 


casc T: 
translate(0.0, -raw_rng * CONVERT_TO_INCHES, -32.0); 
break; 

case 2: 
translate(raw_rng * CONVERT_TO_INCHES,0.0, -32.0 ); 
break; 

case 3: 


if(fabs(auv->dyn.commanded_posture[auv->dyn.posture_no][9]) > 0.04357) 
raw_rng += raw_rng * SIN_S; 
translate(-raw_rng * CONVERT_TO_INCHES,0.0, -32.0 ); 
break; 
case 4: 
translate( 0.0,0.0, -raw_rng * CONVERT_TO_INCHES - 32.0); 
break; 
} 
getmatrix(sonar->sonar_matrix); 
popmatrix(); 


sonar->line_data.sonar_data_pts[sonar->line_data.n_s][0 
sonar->line_data.sonar_data_pts[sonar->linc_data.n_s 
sonar->line_data.sonar_data_pts[sonar->line_data.n_s 


| [0 

| 

| 
sonar->line_data.sonar_data_pts[sonar->line_data.n_s]([ 

| 
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] = зопаг->зопаг_ташх[3][0]: 
| + зопаг->зопаг matrix(3][!]; 
] 2 sonar-»5sonar. matrix(3](2]; 
] = auv->dyn.H_matrix[3] 
] = auv->dyn.H_matrix [3] 
] 3] 


5] = auv->dyn.H_matrix[ 


|: 
| 
|: 


> 


1 
2 
3 
sonar->linec_data.sonar_data_pts[sonar->line_data.n_s][4 
sonar->line_data.sonar_data_pts[sonar->line_data.n_ 
++sonar->line_data.n_s; 
sonar->line_data.n_s = sonar->line_data.n_s % 1500; 


| 
ІП 
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Vector library routines from “GRAPHICS GEMS”, edited by Glassner [Ref. 26] 


i ee r E ы R R алына АН ЛЕ РРА ыы а EE ee ыы ы ы, 


/* return the dot product of vectors a and b */ 
double V3Dot(a, b) 


( 


) 


Vector3 *a, *b; 


return ((a->x * b->x) + (a->y * b->y) + (a->z * b->z)); 


Vector3 *V3Negate(v) 


| 


Vector3 *v: 


V->X =-V->X; 
V->y = -V->y; 
V->Z = -V->Z; 
return (v): 


/* retum vector sum c = a * b */ 
Vector3 *V3Add(a, b, c) 
Vectors “a, "D, "C; 
( 
С->Х = а->Х + ђ->%Х; 
C->y = a->y + b->y; 
С->2 = a->z + b->z; 
return(c); 


) 


/* create, initialize and return a new vector */ 
Vector3 *V3New(a, b, c) 
double a, b, c; 


( 
Vector3 *v = NEWTYPE(Vector3); 


у->х ха; 
v->y = b; 
у->7. = С; 
return (v); 


) 


/* scales the input vector to the new length and returns it */ 
Vector3 * V3MultB yScalar(v, scalar) 
Vector3 *v; 
double scalar; 
| 
v->x *= scalar; 
v->y *= scalar; 
v->z *= scalar; 
return(v); 


) 


/* return vector difference oc = a - b */ 
Vector3 * V3Sub(a, b, c) 
умора на bD, e 
( 
C->X = a->x - b->x; 
C->y =a->y - b->y; 
С->7 = а->7 - ->7; 
return(c); 


| 


/* divides the vector by a scalar and returns it */ 
Vector3 * V3DivideByScalar(v, scalar) 
Vector3 *v; 
double scalar; 
| 
if (scalar != 0.0) 
| 
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v-»x = v->x/scalar; 
v->y = v->y/scalar; 
v-»z - v-»z/scalar; 
return(v); 
} 
} 


/* retums squared length of input vector */ 
double V3SquaredLength(a) 
Vector3 *a; 


| 
) 


return(SQR(a->x) + SQR(a->y) + SQR(a->z)); 


/* returns length of input vector */ 
double V3Length(a) 
Vectora *a; 


( 
) 


return(sqrt(V 3SquaredLength(a))): 


/* normalizes the input vector and returns it */ 
Vector3 * V3Normalize(v) 
Vector3 *v; 
| 
double len = V3Length(v): 
if (len != 0.0) 
| 
у->х /= len; 
v->y /= len; 
v->z /= len: 
return(v); 


| 


/* create, initialize, and return a new vector */ 
Vector3 * V3 Duplicate(a) 
Vector3 *a; 
( 
Vector3 *v = NEWTYPE(Vector3); 


V->X = а->Х; 
у-РУ = а->у; 
V->Z = a->Z; 
return(v); 


| 


/* retum the distance betwqeen two points */ 
double V3DistanceBetweenTwoPoints(a, b) 
Рощиз “а, “5: 
( 

double dx = a->x - b->x; 

double dy = a->y - b-»y; 
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double dz = a->z - b->z; 
return(sqri(SQR(dx) + SQR(dy) + SQR(dz))); 


void V3MultMatrix B yPoint(sonar) 
TRANSDUCER *sonar; 


( 
Vector3 *temp_pt; 


temp_pt = V3New(0.0, 0.0, 0.0); 

temp_pt->x = sonar->sonar_matrix[0][0] * sonar->direction->x + 
sonar->sonar_matrix[0][1] * sonar->direction->y + 
sonar->sonar_matrix[0][2] * sonar->directi9n->z; 

temp_pt->y = sonar->sonar_matrix[1][0] * sonar->direction->x + 
sonar->sonar_matrix[1][1] * sonar->direction->y + 
sonar->sonar_matrix[1][2] * sonar->direction->z; 

temp_pt->z = sonar->sonar_matrix[2][0] * sonar->direction->x + 
sonar-»sonar, matrix[2][1] * sonar->direction->y + 
sonar->sonar_matrix[2][2] * sonar->direction->z; 
sonar->direction->x = temp_pl->x; 

sonar-»direction-»y 2 temp. pt-»y; 

sonar-»direction-»z - temp. pt-»z; 


Jk Ree eS HH RHR HAR HAE ЖК ЖОРЖ ЖКЖ ЖЖЖ A A Ee A EE а O 


This package takes individual range points from sonar_bay.c and 
Computes a least-squares fit line for the points. The line is ended 

for various reasons and a plane is fit to the line, based on the 

computed end points and the width is determined by the range from the 


sonar. Points and planes are passed back to sonar_bay.c for plotting 
TEA IES I AE OK ox Sd т ы 


#include <stdio.h> 
#include <math.h> 
#include “auv.h” 

#include “sonar.h” 


i E ККЕ o o 


line segment init() 
Initialize a line segment and its associated variables/flags 
Called from sonar. rangc() in sonar. bay.c. 
келт ОТТА тт шат 
linc_segment_init(sonar) 
TRANSDUCER *sonar; 


{ 
LINE_SEGMENT *line; 


line = &sonar->line_data; 


if ((line-210c != 1) && (line->loc != 5)) 
convert_coords(line): 
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/* Read in first points to establish initial line segment */ 
line->line_length = 0.0; 
line->bad_pt = FALSE; 


/* accumulate variables */ 
line->sgmx += line->sonar_data_pts[line->n_s][0]; 
line->sgmy += line->sonar_data_pts[line->n_s][1]; 
line->sgmx2 += SQR(line->sonar_data_pts[line->n_s][0]); 
line->sgmy2 += SQR(line->sonar_data_pts[line->n_s][1]); 
line->sgmxy += line->sonar_data_pts[line->n_s][0] * line->sonar_data_pts[line->n_s][1]; 
line->end_pt_no = line->n_s; 
/* Update the counters */ 
if (line->n_s == 99) 

Ппе->п 5-(0; 
else 

++line->n_s; 


if (line->range_pt == 0) 
line->start_pt_no = line->n_s; 


++line->1_s;/* current line segment point counter */ 


if (line->range_pt == MIN_NO_PTS)/* use x data points for first segment */ 
| 
/* Calculate first line segment values */ 
line->mux = line->sgmx / line->1_s; 
line->muy = line->sgmy / line->1_s; 
line->muxx = line->sgmx?2 - (line->sgmx * line->sgmx) / line->1_s: 
line->muyy = line->sgmy?2 - (line->sgmy * line->sgmy) / line->i_s; 
line->muxy = line->sgmxy - (line->sgmx * line->sgmy) / line->1_s; 


line->end_pt_no = line->n_s - 1; 
if (line->end_pt_no < 0) 
line->end_pt_no = line->end_pt_no + 100; 


line->theta_sonar = (atan2(-2.0 * line->muxy, (line->muyy - line->muxx))) / 2.0; 
line->r_sonar = line->mux * cos(line->theta_sonar) + line->muy * sin(line->theta_sonar); 


for (line->j_s = 0; line->j_s < MIN_NO_ PTS; ++line->j_s) 
{ 
line->j_s = (line->j_s + line->start_pt_no) % 100; 
line->sgm_delta_sq += SQR(line->sonar_data_pts[line->j_s][0] - line->mux) 
* SQR(cos(line->theta_sonar)); 
line->sgm_delta_sq += SQR(line->sonar_data_pts[line->j_s][1] - line->muy) 
* SQR(sin(line->theta_sonar)); 
line->sgm_delta_sq += 2.0 * (line->sonar_data_pts[line->j_s][O] - line->mux) 
* (line->sonar_data_pts[line->j_s][1] - line->muy) 
* cos(line->theta_sonar) * sin(line->theta_ sonar); 


109 


) 


[| RH ОЉА ЖЕЖ Еж жж ЖАН А ЕБ ИЕ ат а Ж 
line, seg. compute() 
Read in subsequent data points, after a line segment has been 


initialized and more range values are obtained 
SARS LE LL e mco Жа Т ыт 


line_seg_compute(sonar) 


TRANSDUCER *sonar; 
( 
LINE_SEGMENT *line; 
js line &z sonar-»line. data; */ 


line 2 &sonar-»line. data; 


if ((line-21oc !2 1) && (line->loc != 5)) 
convert. coords(linc); 


/* Calculate test values */ 
line->sigma = line-»sgm delta за / Ппе->1 5; 


/* Test new point for linearity fit */ 
line->delta_line = linc->sonar_data_pts[line->n_s][0] * cos(line->theta_sonar) 
+ line->sonar_data_pts[line->n_s][1] * sin(line->theta_sonar) 
- line->r_sonar; 
if ((fabs(line->delta_line) < (line->sigma * с1)) Il (fabs(line->delta_line) < c2)) 
{ 
line->sgmx += line->sonar_data_pts[line->n_s][0]; 
line->sgmy += line->sonar_data_pts[line->n_s][1); 
line->sgmx2 += SQR(line->sonar_data_pts[line->n_s][0)); 
line->sgmy2 += SQR(line->sonar_data_pts[line->n_s][1]); 
line->sgmxy += line->sonar_data_pts[line->n_s][0] * line->sonar_data_pts[line->n_s][1]; 
line->mux = line->sgmx / (line->1_s + 1); 
line->muy = line->sgmy / (line->1_s + 1); 
line->muxx = linc->sgmx2 - SQR(line->sgmx) / (line->i_s + 1); 
line->muyy = line->sgmy2 - SQR(line->sgmy) / (line->i_s + 1); 
line->muxy = line->sgmxy - (line->sgmx * line->sgmy) / (line->i_s + 1); 


/* calculate ellipse values */ 

line->m_major = (line->muxx + line->muyy) / 2.0 - sqrt((line->muyy - line->muxx) 
* (line->muyy - linec->muxx) / 4.0 + SQR(line->muxy)); 

line->m_minor = (line->muxx + line->muyy) / 2.0 + sqrt((line->muyy - line->muxx) 
* (line->muyy - line->muxx) / 4.0 + SQR(line->muxy)); 

line->d_major = 4.0 * sgri(fabs(line->m_minor / (line->i_s + 1))); 

line->d_minor = 4.0 * sgrt(fabs(line->m_major / (line->1_s + 1))); 
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/* Test new point for ellipse line->thinness */ 
if ((line->d_minor / line->d_major) < c3) 


| 


line->end_pt_no = line->n_s;/* update end point */ 


/* 
* update line segment parameters to include new 
* point 
zi 
line->theta_sonar = (atan2(-2.0 * line-»muxy, (line-»muyy - line-»muxx))) / 2.0; 
line-»r. sonar = line->mux * cos(line->theta_sonar) + line->muy 
* sin(line->theta_sonar); 


line->sgm_delta_sq += 2.0 * (line->sonar_data_pts[line->n_s][Q] - line->mux) 
* (line-»sonar. data, pts[line-»n, s][1] - line-»muy) 
* cos(line-»theta, sonar) * sin(line-»theta, sonar); 

line-»sgm delta sq += SQR(line->sonar_data_pts[line->n_s][1] - line->muy) 
* SQR(sin(line->theta_sonar)); 

line->sgm_delta_sq += SQR(line->sonar_data_pts[line->n_s][0] - line->mux) 
* SQR(cos(line->theta_sonar)): 


if (line->n_s == 99) 
line=>n. s = 5; 
else 
++line->n_s; 
++line->1_s; 
line->delta_x = line->sonar_data_pts[line->start_pt_no][0] 
- line->sonar_data_pts[line->end_pt_no][0]; 
line->delta_y = line->sonar_data_pts[line->start_pt_no][1] 
- line->sonar_data_pts[line->end_pt_no][1]; 
line->line_length = sqrt(SQR(line->delta_x) + SQR(line->delta_y) 
+ SQR(line->sonar_data_pts[line->start_pt_no][3] 
- line->sonar_data_pts[line->end_pt_no][3])); 


/*if (ine->line_length > MAX_LINE_LENGTH) 
| 
line->too_long = TRUE; 
end line, segment(line); 
) 
check_for_termination(line);*/ 
) else 
( 
line->bad_pt = TRUE; 
end line segment(line); 
) 
else 
| 
line->bad_pt = TRUE; 
end line segment(line); 
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ПА А А ИИТИИ ОУ 


end line, segment() 
Wrap up a line segment if bad data pt, course change, depth change, 


or segment max length reached. 
ЖКЖ КК ЖАККА ЖКЖ ЖКЖ ККЖ ККЖ А ВАСИ 


end line segment(line) 
LINE SEGMENT *line; 


int i; 
double line. angle; 


if ((line-»bad. pt 22 TRUE)) 

( 
/* start new line segment */ 
line->sgmx = line->sonar_data_pts[line->n_sJ[0); 
line->sgmy = line->sonar_data_pts[line->n_s][1]; 
line->sgmx2 = SQR(line->sonar_data_pts[line->n_s}[0}); 
line->sgmy2 = SQR(line->sonar_data_pts[line->n_s][1]); 
line->sgmxy = line->sonar_data_pts[line->n_s][0] * linc->sonar_data_pts[line->n_s][1]; 
line->i_s = 1; 
line->sigma = 0; 
line->sgm_delta_sq = 0; 
line->range_pt = 1; 

} else 
line->range_pt = 0; 


/* close out old segment, convert radius to positive value first */ 
if (line->r_sonar < 0) 
( 
line->theta_sonar = 180 * DEG_TO_RAD + line->theta_sonar; 
line->r_sonar = -1 * line-»r. sonar; 
) 
/* determine start and end points on the computed line segment */ 
line->start_pt_x = line->sonar_data_pts[line->start_pt_no][0]; 
line->start_pt_y = line->sonar_data_pts[line->start_pt_no][1]; 
line->end_pt_x = line->sonar_data_pts[line->end_pt_no][0]; 
line->end_pt_y = line->sonar_data_pts[line->end_pt_no][1]; 
line->delta_line = line->start_pt_x * cos(line->theta_sonar) + line->start_pt_y 
* sin(line->theta_sonar) - fabs(line->r_sonar); 
line->start_pt_x = line->start_pt_x - (line->delta_line * cos(line->theta_sonar)); 
line->start_pt_y = line->start_pt_y - (line->delta_line * sin(line->theta_sonar)); 
line->delta_line = line->end_pt_x * cos(line->theta_sonar) + line->end_pt_y * sin(line->theta_sonar) 
- fabs(line->r_sonar); 
line->end_pt_x = line->end_pt_x - (line->delta_line * cos(line->theta_sonar)); 
line->end_pt_y = line->end_pt_y - (line->delta_line * sin(line->theta_sonar)); 
line->delta_x = line->start_pt_x - line->end_pt_x; 


IP 


line->delta_y = line->start_pt_y - line->end_pt_y; 
line->line_length = sqrt(SQR(line->delta_x) + SQR(line->delta_y) 
+ SQR(line->sonar_data_pts[line->start_pt_no][2] 
- line->sonar_data_pts[line->end_pt_no][2])); 


if (line->loc == 1)/* Bottom sonar */ 
( 
line angle 2 atan2(line->sonar_data_pts[line->start_pt_no][2] 
- line-»sonar. data. pts[line-»end, pt no][2], 
line->sonar_data_pts[line->start_pt_no][0] 
- line->sonar_data_pts[line->end_pt_no][0]); 
line. angle 2 line angle - PIOVER2; 
) else 
| 
line_angle = atan2(line->sonar_data_pts[line->start_pt_no][1] 
- line->sonar_data_pts[line->end_pt_no][1], 
sqri(SOR(line-»sonar. data, pts[line-»start, pt. noJ[O] 
- Jine-»sonar. data pts[line-»end pt no][0]) 
+ SQR(line->sonar_data_pts[line->start_pt_no}[2] 
- line->sonar_data_pts[line->end_pt_no][2]))); 
} 
if ((line->loc == 1) Il (line->loc == 5))/* Bottom sonar */ 
{ 
offsetlc 2 line-»sonar. data pts[line-»start, pt. no][3] * 0.0871548 * cos(line. angle); 
offsetls = line->sonar_data_pts[line->start_pt_no][3] * 0.0871548 * sin(line angle); 
offset2c 2 line-»sonar. data, pts[line-»end, pt no][3] * 0.0871548 * cos(line angle); 
offset2s = line->sonar_data_pts[line->end_pt_no][3] * 0.0871548 * sin(line. angle); 
} else 


offsetlc = line->sonar_data_pts[line->start_pt_no][3] * 0.0871548 * cos(line angle); 

offsetls 2 line-»sonar. data pts[line-»start, pt no][3] * 0.0871548 * sin(line. angle); 

offset2c = line-»sonar. data pts[line-»end, pt. noJ[3] * 0.0871548 * cos(line angle); 

offset2s = line->sonar_data_pts[line->end_pt_no][3] * 0.0871548 * sin(line. angle); 
) 


if ((line->line_length >24.0) & & (line->loc != 4)) 
| 
зе рапе ри(Ппе); 
1f (line->n_plane < 99) 
++line->n_plane; 
else 
line->n_plane = 0; 
) 
line->n_s = 0; 
line->start_pt_no = 0; 
Ппе->спа рі по- 0; 


for (1 = 0; 1 < 100; ++1) 
line->sonar_data_pts[i][3] = 0.0; 


if(offsetlc » 48.0) /* limit width of planes to four feet */ 


ІШЕ 


ofísetlc = 48.0; 
if(offsetls > 48.0) 
offsetls = 48.0; 
if(offset2c > 48.0) 
offset2c = 48.0; 
if(offset2s > 48.0) 
offset2s = 48.0; 


) 


ПА А А ЈА АЈА А ДАО КАЗАО А 


convert_coords() 
ЖАЗ ЖОК ЖАКЕ ЖАКЕ ЖК КЖ А ЖЖ ЖА ЖОН ena f 
convert_coords(line) 
LINE_SEGMENT *line; 


double temp_val: 


/* 
* store z data into y var, y into z for bottom sonar 


ЕТ 


temp. уа! - line->sonar_data_pts[line->n_s][2); 
line->sonar_data_pts[line->n_s][2] = line->sonar_data_pts[line->n_s][1); 
line->sonar_data_pts[line->n_s][1] = temp_val; 


) 


ПАА 


check_for_terrmination() 
жи опре тесла лале A | 
check_for_termination(line) 
LINE_SEGMENT *line; 
( 
if (sqrt(SQR(ine->sonar_data_pts[{line->start_pt_no][{0] 
- line->sonar_data_pts[line->end_pt_no]{0]) 
+ SQR(line->sonar_data_pts[line->start_pi_noJ[1] 
- line-»sonar. data, pts(line-»end. pt no][1]) 
+ SQR(line->sonar_data_pts[line->start_pt_no][2] 
- line->sonar_data_pts{line->end_pt_no][2])) 
> MAX_LINE_LENGTH) 
{ 
line->end_pt_no = line->end_pt_no - 1; 
line->too_long = TRUE; 


) 
if ((line->loc == 1) Il (line->loc == 4)) 
{ 
if (fabs(line->sonar_data_pts[line->start_pt_no][4] 
- line->sonar_data_pts[line->end_pt_no][4]) 
> MAX_COURSE_CHANGE) 
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line->course_change = TRUE; 


) 
Velse 
| 
if (fabs(line->sonar_data_pts[line->start_pt_no][4] 
- line->sonar_data_pts[line->end_pt_no][4]) 
> MAX_DEPTH_CHANGE) 


( 
line->depth_change = TRUE; 


| 

| 

if ((line-2too. long) Il (line-»course. change) ll (line-»depth, change)) 
end line segment(line); 


| 


E uote DETTO STUART AE T EE ETE nro mms 


set. plane. ptr() 
Store the plane data points into the array for display. 
Sd JE 


set plane pur(line) 
LINE SEGMENT *line; 


int 1; 
double line. angle; 


if ((line->loc == 1) Il (line->loc == 5)) 

( 
line->plane_pts[line->n_plane][0][0] = line->sonar_data_pts[line->start_pt_no)][0] * offsetlc: 
line->plane_pts[line->n_plane][0][1] = line->sonar_data_pts[line->start_pt_noJ[1); 
line->plane_pts[line->n_plane][0][2] = line->sonar_data_pts[line->start_pt_no][2] + offsctls; 
line->plane_pts[line->n_plane][1][0] = line->sonar_data_pts[line->start_pt_no][O] - offsetic; 


line->plane_pts[line->n_plane]f{1][1] = line->sonar_data_pts[line->start_pt_no][1]; 
line->plane_pts[line->n_plane][1][2] 2 line-»sonar. data pts[line-»start, pt. noJ[2] - offsetls; 
line->plane_pts[line->n_plane][2][0] = line->sonar_data_pts[line->end_pt_no][0] + offsct2c; 


line->plane_pts[line->n_planc][2][1] = line->sonar_data_pts[line->cnd_pt_no][1]; 
line->plane_pts[line->n_plane][2][2] = line->sonar_data_pts[line->end_pt_no][2] + offsct2s: 
line->plane_pts{line->n_plane][3][0] 2 line-»sonar. data. pts[line-»end. pt. no]J[O] - offset2c; 


line-»plane pts[line-»n. planc][3][1] 2 line-5sonar. data. pts[linc-»end, pt. no][1]; 
line->plane_pts{line->n_plane][3][2] = line-»sonar. data. pts(line-»end pt noj[2] - offscO2s; 
) else 
( 
line->plane_pts[line->n_plane][0][0] = line->sonar_data_pts[line->start_pi_no][0] 
* offsetlc * cos(linc-»theta. sonar); 
line->plane_pts[line->n_plane][(QJ[1] = line->sonar_data_pts[line->start_pt_no][2] + oflsetls 
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offsetlc; 

line-»plane pts[line-»n. plane][0][2] 2 line-2sonar. data pts[line-»start. pt no][1]; 

line->plane_pts[line->n_plane][1][0] = line->sonar_data_pts[line->start_pt_no}[0] 
* offsetlc * cos(line-»theta sonar); 

line-»plane. pis(linc-»n. planc][1][1] = linc->sonar_data_pts[line->start_pt_no]|[2] 
- offsetls - offsetlc; 

line-»plane pts[line-»n planej[1][2] = line->sonar_data_pts[line->start_pt_noJ[1]; 

line-»plane pts[linc-»n. plane][2][0] 2 ппе->ѕопаг data. pts[line-»end pt noj[0] 
* oífsetlc * cos(line-»theta sonar); 

line->plane_pts[line->n_planeJ][2][1] = line->sonar_data_pts[line->end_pt_no][2] 
+ offset2s + offset2c; 

line->plane_pts[line->n_plane][2][2] = line->sonar_data_pts[line->end_pt_no][1]; 

line->plane_pts[line->n_plane][3][0] = line->sonar_data_pts[line->end_pt_no][0] 
+ offsetlc * cos(line->theta_sonar); 

line->plane_pts[line->n_plane][3][1] = line->sonar_data_pts[line->end_pt_no][2] 
- offset2s - offset2c; 

line->plane_pts[line->n_plane][3](2] = line->sonar_data_pts[line->end_pt_no][1]; 
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